Added ecrt_slave_config_idn() method to store SoE IDN configurations.

NEWS

@@ -13,6 +13,7 @@ Changes since 1.4.0:
 * Added distributed clocks support.
 * Added watchdog configuration via application interface (thanks to J. Mohre).
 * Added VoE mailbox protocol support.
+* Added SoE mailbox protocol support.
 * Improved the callback mechanism. ecrt_master_callbacks() now takes two
   callback functions for sending and receiving datagrams.
   ecrt_master_send_ext() is used to execute the sending of non-application

include/ecrt.h

@@ -75,6 +75,7 @@
  * - Removed 'const' from argument of ecrt_sdo_request_state(), because the
  *   userspace library has to modify object internals.
  * - Added 64-bit data access macros.
+ * - Added ecrt_slave_config_idn() method for storing SoE IDN configurations.
  *
  * @{
  */
@@ -1134,6 +1135,27 @@ void ecrt_slave_config_state(
         ec_slave_config_state_t *state /**< State object to write to. */
         );
 
+/** Add an SoE IDN configuration.
+ *
+ * A configuration for a Sercos-over-EtherCAT IDN is stored in the slave
+ * configuration object and is written to the slave whenever the slave is
+ * being configured by the master. This usually happens once on master
+ * activation, but can be repeated subsequently, for example after the slave's
+ * power supply failed.
+ *
+ * Please note that the this function does not do any endianess correction.
+ * Data have to be passed in EtherCAT endianess (little-endian).
+ *
+ * \retval  0 Success.
+ * \retval <0 Error code.
+ */
+int ecrt_slave_config_idn(
+        ec_slave_config_t *sc, /**< Slave configuration. */
+        uint16_t idn, /**< SoE IDN. */
+        const uint8_t *data, /**< Pointer to the data. */
+        size_t size /**< Size of the \a data. */
+        );
+
 /******************************************************************************
  * Domain methods
  *****************************************************************************/

master/fsm_slave_config.c

@@ -54,6 +54,7 @@ void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
@@ -71,6 +72,7 @@ void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);
@@ -113,6 +115,7 @@ void ec_fsm_slave_config_clear(
         )
 {
     ec_sdo_request_clear(&fsm->request_copy);
+    ec_soe_request_clear(&fsm->soe_request_copy);
 }
 
 /*****************************************************************************/
@@ -759,7 +762,7 @@ void ec_fsm_slave_config_enter_sdo_conf(
 
     // No CoE configuration to be applied?
     if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration
-        ec_fsm_slave_config_enter_pdo_conf(fsm);
+        ec_fsm_slave_config_enter_soe_conf(fsm);
         return;
     }
 
@@ -807,6 +810,81 @@ void ec_fsm_slave_config_state_sdo_conf(
         return;
     }
 
+    // All SDOs are now configured.
+	ec_fsm_slave_config_enter_soe_conf(fsm);
+}
+
+/*****************************************************************************/
+
+/** Check for SoE configurations to be applied.
+ */
+void ec_fsm_slave_config_enter_soe_conf(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+	ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
+
+    if (!slave->config) {
+        ec_fsm_slave_config_enter_pdo_sync(fsm);
+        return;
+    }
+
+    // No SoE configuration to be applied?
+    if (list_empty(&slave->config->soe_configs)) { // skip configuration
+        ec_fsm_slave_config_enter_pdo_conf(fsm);
+        return;
+    }
+
+    // start SoE configuration
+    fsm->state = ec_fsm_slave_config_state_soe_conf;
+    fsm->soe_request = list_entry(fsm->slave->config->soe_configs.next,
+            ec_soe_request_t, list);
+    ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+	ec_soe_request_write(&fsm->soe_request_copy);
+    ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
+    ec_fsm_soe_exec(fsm_soe); // execute immediately
+}
+
+/*****************************************************************************/
+
+/** Slave configuration state: SOE_CONF.
+ */
+void ec_fsm_slave_config_state_soe_conf(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+	ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
+
+    if (ec_fsm_soe_exec(fsm_soe)) {
+		return;
+	}
+
+    if (!ec_fsm_soe_success(fsm_soe)) {
+        EC_ERR("SoE configuration failed for slave %u.\n",
+                fsm->slave->ring_position);
+        fsm->slave->error_flag = 1;
+        fsm->state = ec_fsm_slave_config_state_error;
+        return;
+    }
+
+    if (!fsm->slave->config) { // config removed in the meantime
+        ec_fsm_slave_config_reconfigure(fsm);
+        return;
+    }
+
+    // Another IDN to configure?
+    if (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) {
+        fsm->soe_request = list_entry(fsm->soe_request->list.next,
+                ec_soe_request_t, list);
+        ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+        ec_soe_request_write(&fsm->soe_request_copy);
+        ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
+        ec_fsm_soe_exec(fsm_soe); // execute immediately
+        return;
+    }
+
     // All SDOs are now configured.
     ec_fsm_slave_config_enter_pdo_conf(fsm);
 }

master/fsm_slave_config.h

@@ -63,6 +63,8 @@ struct ec_fsm_slave_config
     unsigned int retries; /**< Retries on datagram timeout. */
     ec_sdo_request_t *request; /**< SDO request for SDO configuration. */
     ec_sdo_request_t request_copy; /**< Copied SDO request. */
+    ec_soe_request_t *soe_request; /**< SDO request for SDO configuration. */
+    ec_soe_request_t soe_request_copy; /**< Copied SDO request. */
     unsigned long jiffies_start; /**< For timeout calculations. */
     unsigned int take_time; /**< Store jiffies after datagram reception. */
 };

master/slave_config.c

@@ -86,6 +86,7 @@ void ec_slave_config_init(
     INIT_LIST_HEAD(&sc->sdo_configs);
     INIT_LIST_HEAD(&sc->sdo_requests);
     INIT_LIST_HEAD(&sc->voe_handlers);
+    INIT_LIST_HEAD(&sc->soe_configs);
 }
 
 /*****************************************************************************/
@@ -101,6 +102,7 @@ void ec_slave_config_clear(
     unsigned int i;
     ec_sdo_request_t *req, *next_req;
     ec_voe_handler_t *voe, *next_voe;
+    ec_soe_request_t *soe, *next_soe;
 
     ec_slave_config_detach(sc);
 
@@ -128,6 +130,13 @@ void ec_slave_config_clear(
         ec_voe_handler_clear(voe);
         kfree(voe);
     }
+
+    // free all SoE configurations
+    list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) {
+        list_del(&soe->list);
+        ec_soe_request_clear(soe);
+        kfree(soe);
+    }
 }
 
 /*****************************************************************************/
@@ -950,6 +959,46 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
 
 /*****************************************************************************/
 
+int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
+		const uint8_t *data, size_t size)
+{
+    ec_slave_t *slave = sc->slave;
+    ec_soe_request_t *req;
+    int ret;
+
+    if (sc->master->debug_level)
+        EC_DBG("ecrt_slave_config_idn(sc = 0x%p, idn = 0x%04X, "
+                "data = 0x%p, size = %zu)\n", sc, idn, data, size);
+
+    if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+        EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);
+        return -EPROTONOSUPPORT; // protocol not supported
+    }
+
+    if (!(req = (ec_soe_request_t *)
+          kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) {
+        EC_ERR("Failed to allocate memory for IDN configuration!\n");
+        return -ENOMEM;
+    }
+
+    ec_soe_request_init(req);
+    ec_soe_request_set_idn(req, idn);
+
+    ret = ec_soe_request_copy_data(req, data, size);
+    if (ret < 0) {
+        ec_soe_request_clear(req);
+        kfree(req);
+        return ret;
+    }
+        
+    down(&sc->master->master_sem);
+    list_add_tail(&req->list, &sc->soe_configs);
+    up(&sc->master->master_sem);
+    return 0;
+}
+
+/*****************************************************************************/
+
 /** \cond */
 
 EXPORT_SYMBOL(ecrt_slave_config_sync_manager);
@@ -969,6 +1018,7 @@ EXPORT_SYMBOL(ecrt_slave_config_complete_sdo);
 EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request);
 EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler);
 EXPORT_SYMBOL(ecrt_slave_config_state);
+EXPORT_SYMBOL(ecrt_slave_config_idn);
 
 /** \endcond */
 

master/slave_config.h

@@ -77,6 +77,7 @@ struct ec_slave_config {
     struct list_head sdo_configs; /**< List of SDO configurations. */
     struct list_head sdo_requests; /**< List of SDO requests. */
     struct list_head voe_handlers; /**< List of VoE handlers. */
+    struct list_head soe_configs; /**< List of SoE configurations. */
 };
 
 /*****************************************************************************/