From 7facae8e3bfe5dccfec1644c44b5d1c486a7aead Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Fri, 23 Jul 2021 16:16:18 +0200
Subject: [PATCH] First feature flag: AssignToPdi.

---
 master/fsm_master.c       | 62 +++++++++++++++++++++++++++++++++++++++
 master/fsm_slave_config.c | 36 +++++++++++++++++++----
 2 files changed, 93 insertions(+), 5 deletions(-)

diff --git a/master/fsm_master.c b/master/fsm_master.c
index 9b07796e..c62d328f 100644
--- a/master/fsm_master.c
+++ b/master/fsm_master.c
@@ -69,6 +69,7 @@ void ec_fsm_master_state_dc_measure_delays(ec_fsm_master_t *);
 void ec_fsm_master_state_scan_slave(ec_fsm_master_t *);
 void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *);
 void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *);
+void ec_fsm_master_state_assign_sii(ec_fsm_master_t *);
 void ec_fsm_master_state_write_sii(ec_fsm_master_t *);
 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);
 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);
@@ -449,6 +450,9 @@ int ec_fsm_master_action_process_sii(
 {
     ec_master_t *master = fsm->master;
     ec_sii_write_request_t *request;
+    ec_slave_config_t *config;
+    ec_flag_t *flag;
+    int assign_to_pdi;
 
     // search the first request to be processed
     while (1) {
@@ -461,6 +465,27 @@ int ec_fsm_master_action_process_sii(
         list_del_init(&request->list); // dequeue
         request->state = EC_INT_REQUEST_BUSY;
 
+        assign_to_pdi = 0;
+        config = request->slave->config;
+        if (config) {
+            flag = ec_slave_config_find_flag(config, "AssignToPdi");
+            if (flag) {
+                assign_to_pdi = flag->value;
+            }
+        }
+
+        if (assign_to_pdi) {
+            fsm->sii_request = request;
+            EC_SLAVE_DBG(request->slave, 1,
+                    "Assigning SII back to EtherCAT.\n");
+            ec_datagram_fpwr(fsm->datagram, request->slave->station_address,
+                    0x0500, 0x01);
+            EC_WRITE_U8(fsm->datagram->data, 0x00); // EtherCAT
+            fsm->retries = EC_FSM_RETRIES;
+            fsm->state = ec_fsm_master_state_assign_sii;
+            return 1;
+        }
+
         // found pending SII write operation. execute it!
         EC_SLAVE_DBG(request->slave, 1, "Writing SII data...\n");
         fsm->sii_request = request;
@@ -1413,6 +1438,43 @@ void ec_fsm_master_state_dc_write_offset(
 
 /*****************************************************************************/
 
+/** Master state: ASSIGN SII.
+ */
+void ec_fsm_master_state_assign_sii(
+        ec_fsm_master_t *fsm /**< Master state machine. */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_sii_write_request_t *request = fsm->sii_request;
+    ec_slave_t *slave = request->slave;
+
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        EC_SLAVE_ERR(slave, "Failed to receive SII assignment datagram: ");
+        ec_datagram_print_state(datagram);
+        goto cont;
+    }
+
+    if (datagram->working_counter != 1) {
+        EC_SLAVE_ERR(slave, "Failed to assign SII back to EtherCAT: ");
+        ec_datagram_print_wc_error(datagram);
+        goto cont;
+    }
+
+cont:
+    // found pending SII write operation. execute it!
+    EC_SLAVE_DBG(slave, 1, "Writing SII data (after assignment)...\n");
+    fsm->sii_index = 0;
+    ec_fsm_sii_write(&fsm->fsm_sii, slave, request->offset,
+            request->words, EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+    fsm->state = ec_fsm_master_state_write_sii;
+    fsm->state(fsm); // execute immediately
+}
+
+/*****************************************************************************/
+
 /** Master state: WRITE SII.
  */
 void ec_fsm_master_state_write_sii(
diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c
index 704bf7aa..49d916f4 100755
--- a/master/fsm_slave_config.c
+++ b/master/fsm_slave_config.c
@@ -703,6 +703,11 @@ void ec_fsm_slave_config_state_boot_preop(
         )
 {
     ec_slave_t *slave = fsm->slave;
+#ifdef EC_SII_ASSIGN
+    int assign_to_pdi;
+    ec_slave_config_t *config;
+    ec_flag_t *flag;
+#endif
 
     if (ec_fsm_change_exec(fsm->fsm_change)) {
         return;
@@ -722,12 +727,33 @@ void ec_fsm_slave_config_state_boot_preop(
             slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT");
 
 #ifdef EC_SII_ASSIGN
-    EC_SLAVE_DBG(slave, 1, "Assigning SII access back to EtherCAT.\n");
+    assign_to_pdi = 0;
+    config = fsm->slave->config;
+    if (config) {
+        flag = ec_slave_config_find_flag(config, "AssignToPdi");
+        if (flag) {
+            assign_to_pdi = flag->value;
+        }
+    }
 
-    ec_datagram_fpwr(fsm->datagram, slave->station_address, 0x0500, 0x01);
-    EC_WRITE_U8(fsm->datagram->data, 0x00); // EtherCAT
-    fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_slave_config_state_assign_ethercat;
+    if (assign_to_pdi) {
+        EC_SLAVE_DBG(slave, 1, "Skipping SII assignment back to EtherCAT.\n");
+        if (slave->current_state == slave->requested_state) {
+            fsm->state = ec_fsm_slave_config_state_end; // successful
+            EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");
+            return;
+        }
+
+        ec_fsm_slave_config_enter_sdo_conf(fsm);
+    }
+    else {
+        EC_SLAVE_DBG(slave, 1, "Assigning SII access back to EtherCAT.\n");
+
+        ec_datagram_fpwr(fsm->datagram, slave->station_address, 0x0500, 0x01);
+        EC_WRITE_U8(fsm->datagram->data, 0x00); // EtherCAT
+        fsm->retries = EC_FSM_RETRIES;
+        fsm->state = ec_fsm_slave_config_state_assign_ethercat;
+    }
 #else
     if (slave->current_state == slave->requested_state) {
         fsm->state = ec_fsm_slave_config_state_end; // successful