update: replace slave with master+slave_index

Signed-off-by: sakumisu <1203593632@qq.com>

include/ec_coe.h

@@ -6,7 +6,8 @@
 #ifndef EC_COE_H
 #define EC_COE_H
 
-int ec_coe_download(ec_slave_t *slave,
+int ec_coe_download(ec_master_t *master,
+                    uint16_t slave_index,
                     ec_datagram_t *datagram,
                     uint16_t index,
                     uint8_t subindex,
@@ -14,7 +15,8 @@ int ec_coe_download(ec_slave_t *slave,
                     uint32_t size,
                     bool complete_access);
 
-int ec_coe_upload(ec_slave_t *slave,
+int ec_coe_upload(ec_master_t *master,
+                  uint16_t slave_index,
                   ec_datagram_t *datagram,
                   uint16_t index,
                   uint8_t subindex,

include/ec_foe.h

@@ -6,14 +6,16 @@
 #ifndef EC_FOE_H
 #define EC_FOE_H
 
-int ec_foe_write(ec_slave_t *slave,
+int ec_foe_write(ec_master_t *master,
+                 uint16_t slave_index,
                  ec_datagram_t *datagram,
                  const char *filename,
                  uint32_t password,
                  const void *buf,
                  uint32_t size);
 
-int ec_foe_read(ec_slave_t *slave,
+int ec_foe_read(ec_master_t *master,
+                uint16_t slave_index,
                 ec_datagram_t *datagram,
                 const char *filename,
                 uint32_t password,

include/ec_mailbox.h

@@ -6,9 +6,23 @@
 #ifndef EC_MAILBOX_H
 #define EC_MAILBOX_H
 
-uint8_t *ec_mailbox_fill_send(ec_slave_t *slave, ec_datagram_t *datagram, uint8_t type, uint16_t size);
-int ec_mailbox_send(ec_slave_t *slave, ec_datagram_t *datagram);
-int ec_mailbox_read_status(ec_slave_t *slave, ec_datagram_t *datagram, uint32_t timeout_us);
-int ec_mailbox_receive(ec_slave_t *slave, ec_datagram_t *datagram, uint8_t *type, uint32_t *size, uint32_t timeout_us);
+uint8_t *ec_mailbox_fill_send(ec_master_t *master,
+                              uint16_t slave_index,
+                              ec_datagram_t *datagram,
+                              uint8_t type,
+                              uint16_t size);
+int ec_mailbox_send(ec_master_t *master,
+                    uint16_t slave_index,
+                    ec_datagram_t *datagram);
+int ec_mailbox_read_status(ec_master_t *master,
+                           uint16_t slave_index,
+                           ec_datagram_t *datagram,
+                           uint32_t timeout_us);
+int ec_mailbox_receive(ec_master_t *master,
+                       uint16_t slave_index,
+                       ec_datagram_t *datagram,
+                       uint8_t *type,
+                       uint32_t *size,
+                       uint32_t timeout_us);
 
 #endif

include/ec_sii.h

@@ -34,7 +34,7 @@ typedef struct ec_sii {
     uint32_t string_count; /**< Number of SII strings. */
 } ec_sii_t;
 
-int ec_sii_read(ec_slave_t *slave, ec_datagram_t *datagram, uint16_t woffset, uint32_t *buf, uint32_t len);
-int ec_sii_write(ec_slave_t *slave, ec_datagram_t *datagram, uint16_t woffset, const uint16_t *buf, uint32_t len);
+int ec_sii_read(ec_master_t *master, uint16_t slave_index, ec_datagram_t *datagram, uint16_t woffset, uint32_t *buf, uint32_t len);
+int ec_sii_write(ec_master_t *master, uint16_t slave_index, ec_datagram_t *datagram, uint16_t woffset, const uint16_t *buf, uint32_t len);
 
 #endif

src/ec_cmd.c

@@ -771,12 +771,10 @@ int ethercat(int argc, const char **argv)
         int ret;
 
         uint32_t slave_idx = atoi(argv[3]);
-        if (slave_idx >= global_cmd_master->slave_count) {
-            EC_LOG_RAW("No slaves found\n");
-            return -1;
-        }
+
         ec_datagram_init(&datagram, 512);
-        ret = ec_coe_upload(&global_cmd_master->slaves[slave_idx],
+        ret = ec_coe_upload(global_cmd_master,
+                            slave_idx,
                             &datagram,
                             strtoul(argv[4], NULL, 16),
                             argc >= 6 ? strtoul(argv[5], NULL, 16) : 0x00,
@@ -800,10 +798,6 @@ int ethercat(int argc, const char **argv)
         int ret;
 
         uint32_t slave_idx = atoi(argv[3]);
-        if (slave_idx >= global_cmd_master->slave_count) {
-            EC_LOG_RAW("No slaves found\n");
-            return -1;
-        }
         u32data = strtoul(argv[6], NULL, 16);
 
         if (u32data < 0xff)
@@ -814,7 +808,8 @@ int ethercat(int argc, const char **argv)
             size = 4;
 
         ec_datagram_init(&datagram, 512);
-        ret = ec_coe_download(&global_cmd_master->slaves[slave_idx],
+        ret = ec_coe_download(global_cmd_master,
+                              slave_idx,
                               &datagram,
                               strtoul(argv[4], NULL, 16),
                               strtoul(argv[5], NULL, 16),
@@ -842,21 +837,24 @@ int ethercat(int argc, const char **argv)
         return 0;
     } else if (argc >= 5 && strcmp(argv[1], "sii_write") == 0) {
         // ethercat sii_write -p [slave_idx]
-        uint32_t slave_idx = atoi(argv[3]);
-        if (slave_idx >= global_cmd_master->slave_count) {
-            EC_LOG_RAW("No slaves found\n");
-            return -1;
-        }
+        int ret;
         static ec_datagram_t datagram;
+        extern unsigned char cherryecat_eepromdata[2048];
 
         ec_datagram_init(&datagram, 4096);
 
-        extern unsigned char cherryecat_eepromdata[2048];
+        uint32_t slave_idx = atoi(argv[3]);
 
         ec_osal_mutex_take(global_cmd_master->scan_lock);
-        ec_sii_write(&global_cmd_master->slaves[slave_idx], &datagram, 0x0000, (const uint16_t *)cherryecat_eepromdata, sizeof(cherryecat_eepromdata));
+        ret = ec_sii_write(global_cmd_master, slave_idx, &datagram, 0x0000, (const uint16_t *)cherryecat_eepromdata, sizeof(cherryecat_eepromdata));
         ec_osal_mutex_give(global_cmd_master->scan_lock);
 
+        if (ret < 0) {
+            EC_SLAVE_LOG_ERR("Slave %u sii_write failed: %d\n", slave_idx, ret);
+        } else {
+            EC_LOG_RAW("Slave %u sii write success\n", slave_idx);
+        }
+
         ec_datagram_clear(&datagram);
         return 0;
     } else if (argc >= 2 && strcmp(argv[1], "pdo_read") == 0) {
@@ -952,10 +950,6 @@ int ethercat(int argc, const char **argv)
         uint32_t size;
         int ret;
         uint32_t slave_idx = atoi(argv[3]);
-        if (slave_idx >= global_cmd_master->slave_count) {
-            EC_LOG_RAW("No slaves found\n");
-            return -1;
-        }
         const char *filename = argv[4];
         uint32_t password = strtoul(argv[5], NULL, 16);
 
@@ -970,7 +964,7 @@ int ethercat(int argc, const char **argv)
         EC_SLAVE_LOG_INFO("Slave %u foe write file %s, password: 0x%08x, size %u\n", slave_idx, filename, password, size);
 
         ec_osal_mutex_take(global_cmd_master->scan_lock);
-        ret = ec_foe_write(&global_cmd_master->slaves[slave_idx], &datagram, filename, password, hexdata, size);
+        ret = ec_foe_write(global_cmd_master, slave_idx, &datagram, filename, password, hexdata, size);
         ec_osal_mutex_give(global_cmd_master->scan_lock);
 
         if (ret < 0) {
@@ -987,10 +981,6 @@ int ethercat(int argc, const char **argv)
         uint32_t size;
         int ret;
         uint32_t slave_idx = atoi(argv[3]);
-        if (slave_idx >= global_cmd_master->slave_count) {
-            EC_LOG_RAW("No slaves found\n");
-            return -1;
-        }
         const char *filename = argv[4];
         uint32_t password = strtoul(argv[5], NULL, 16);
 
@@ -1001,7 +991,7 @@ int ethercat(int argc, const char **argv)
         ec_datagram_init(&datagram, 4096);
 
         ec_osal_mutex_take(global_cmd_master->scan_lock);
-        ret = ec_foe_read(&global_cmd_master->slaves[slave_idx], &datagram, filename, password, hexdata, sizeof(hexdata), &size);
+        ret = ec_foe_read(global_cmd_master, slave_idx, &datagram, filename, password, hexdata, sizeof(hexdata), &size);
         ec_osal_mutex_give(global_cmd_master->scan_lock);
 
         if (ret < 0) {

src/ec_coe.c

@@ -23,11 +23,11 @@ typedef ec_coe_download_segment_header_t ec_coe_upload_segment_header_t;
 
 /** CoE download request header size.
  */
-#define EC_COE_DOWN_REQ_HEADER_SIZE     (sizeof(ec_coe_download_common_header_t))
+#define EC_COE_DOWN_REQ_HEADER_SIZE (sizeof(ec_coe_download_common_header_t))
 
 /** CoE upload request header size.
  */
-#define EC_COE_UP_REQ_HEADER_SIZE       (sizeof(ec_coe_upload_common_header_t))
+#define EC_COE_UP_REQ_HEADER_SIZE (sizeof(ec_coe_upload_common_header_t))
 
 /** CoE download segment request header size.
  */
@@ -35,15 +35,16 @@ typedef ec_coe_download_segment_header_t ec_coe_upload_segment_header_t;
 
 /** CoE upload segment request header size.
  */
-#define EC_COE_UP_SEG_REQ_HEADER_SIZE   (sizeof(ec_coe_upload_segment_header_t))
+#define EC_COE_UP_SEG_REQ_HEADER_SIZE (sizeof(ec_coe_upload_segment_header_t))
 
 /** Minimum size of download segment.
  */
-#define EC_COE_DOWN_SEG_MIN_DATA_SIZE   7
+#define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7
 
-#define EC_COE_TIMEOUT_US          (1000 * 1000) /* 1s */
+#define EC_COE_TIMEOUT_US (1000 * 1000) /* 1s */
 
-static int ec_coe_download_expedited(ec_slave_t *slave,
+static int ec_coe_download_expedited(ec_master_t *master,
+                                     uint16_t slave_index,
                                      ec_datagram_t *datagram,
                                      uint16_t index,
                                      uint8_t subindex,
@@ -57,7 +58,7 @@ static int ec_coe_download_expedited(ec_slave_t *slave,
     ec_coe_download_common_header_t *download_common;
     int ret;
 
-    data = ec_mailbox_fill_send(slave, datagram, EC_MBOX_TYPE_COE, EC_COE_DOWN_REQ_HEADER_SIZE);
+    data = ec_mailbox_fill_send(master, slave_index, datagram, EC_MBOX_TYPE_COE, EC_COE_DOWN_REQ_HEADER_SIZE);
 
     download_common = (ec_coe_download_common_header_t *)data;
     download_common->coe_header.number = 0;
@@ -74,12 +75,12 @@ static int ec_coe_download_expedited(ec_slave_t *slave,
 
     ec_memcpy(download_common->data, buf, size);
     memset(download_common->data + size, 0x00, 4 - size);
-    ret = ec_mailbox_send(slave, datagram);
+    ret = ec_mailbox_send(master, slave_index, datagram);
     if (ret < 0) {
         return ret;
     }
 
-    ret = ec_mailbox_receive(slave, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
+    ret = ec_mailbox_receive(master, slave_index, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
     if (ret < 0) {
         return ret;
     }
@@ -96,7 +97,7 @@ static int ec_coe_download_expedited(ec_slave_t *slave,
 
     if (EC_READ_U16(data) >> 12 == EC_COE_SERVICE_SDO_REQUEST &&
         EC_READ_U8(data + 2) >> 5 == EC_COE_REQUEST_ABORT) {
-        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave->index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
+        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave_index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
         return -EC_ERR_COE_ABORT;
     }
 
@@ -110,7 +111,8 @@ static int ec_coe_download_expedited(ec_slave_t *slave,
     return 0;
 }
 
-static int ec_coe_download_common(ec_slave_t *slave,
+static int ec_coe_download_common(ec_master_t *master,
+                                  uint16_t slave_index,
                                   ec_datagram_t *datagram,
                                   uint16_t index,
                                   uint8_t subindex,
@@ -120,15 +122,11 @@ static int ec_coe_download_common(ec_slave_t *slave,
 {
     uint8_t *data;
     uint8_t mbox_proto;
-    uint32_t data_size, recv_size, max_data_size;
+    uint32_t recv_size;
     ec_coe_download_common_header_t *download_common;
     int ret;
 
-    max_data_size = slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_COE_DOWN_REQ_HEADER_SIZE;
-
-    data_size = MIN(size, max_data_size);
-
-    data = ec_mailbox_fill_send(slave, datagram, EC_MBOX_TYPE_COE, data_size + EC_COE_DOWN_REQ_HEADER_SIZE);
+    data = ec_mailbox_fill_send(master, slave_index, datagram, EC_MBOX_TYPE_COE, size + EC_COE_DOWN_REQ_HEADER_SIZE);
 
     download_common = (ec_coe_download_common_header_t *)data;
     download_common->coe_header.number = 0;
@@ -144,13 +142,13 @@ static int ec_coe_download_common(ec_slave_t *slave,
     download_common->subindex = complete_access ? 0x00 : subindex;
 
     ec_memcpy(download_common->data, &size, 4);
-    ec_memcpy(data + EC_COE_DOWN_REQ_HEADER_SIZE, buf, data_size);
-    ret = ec_mailbox_send(slave, datagram);
+    ec_memcpy(data + EC_COE_DOWN_REQ_HEADER_SIZE, buf, size);
+    ret = ec_mailbox_send(master, slave_index, datagram);
     if (ret < 0) {
         return ret;
     }
 
-    ret = ec_mailbox_receive(slave, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
+    ret = ec_mailbox_receive(master, slave_index, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
     if (ret < 0) {
         return ret;
     }
@@ -167,7 +165,7 @@ static int ec_coe_download_common(ec_slave_t *slave,
 
     if (EC_READ_U16(data) >> 12 == EC_COE_SERVICE_SDO_REQUEST &&
         EC_READ_U8(data + 2) >> 5 == EC_COE_REQUEST_ABORT) {
-        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave->index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
+        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave_index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
         return -EC_ERR_COE_ABORT;
     }
 
@@ -181,7 +179,8 @@ static int ec_coe_download_common(ec_slave_t *slave,
     return 0;
 }
 
-static int ec_coe_download_segment(ec_slave_t *slave,
+static int ec_coe_download_segment(ec_master_t *master,
+                                   uint16_t slave_index,
                                    ec_datagram_t *datagram,
                                    const void *seg_data,
                                    uint16_t size,
@@ -203,7 +202,7 @@ static int ec_coe_download_segment(ec_slave_t *slave,
         data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE;
     }
 
-    data = ec_mailbox_fill_send(slave, datagram, EC_MBOX_TYPE_COE, data_size + EC_COE_DOWN_SEG_REQ_HEADER_SIZE);
+    data = ec_mailbox_fill_send(master, slave_index, datagram, EC_MBOX_TYPE_COE, data_size + EC_COE_DOWN_SEG_REQ_HEADER_SIZE);
 
     download_seg = (ec_coe_download_segment_header_t *)data;
     download_seg->coe_header.number = 0;
@@ -220,12 +219,12 @@ static int ec_coe_download_segment(ec_slave_t *slave,
         memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + size, 0x00, EC_COE_DOWN_SEG_MIN_DATA_SIZE - size);
     }
 
-    ret = ec_mailbox_send(slave, datagram);
+    ret = ec_mailbox_send(master, slave_index, datagram);
     if (ret < 0) {
         return ret;
     }
 
-    ret = ec_mailbox_receive(slave, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
+    ret = ec_mailbox_receive(master, slave_index, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
     if (ret < 0) {
         return ret;
     }
@@ -242,7 +241,7 @@ static int ec_coe_download_segment(ec_slave_t *slave,
 
     if (EC_READ_U16(data) >> 12 == EC_COE_SERVICE_SDO_REQUEST &&
         EC_READ_U8(data + 2) >> 5 == EC_COE_REQUEST_ABORT) {
-        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave->index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
+        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave_index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
         return -EC_ERR_COE_ABORT;
     }
 
@@ -258,7 +257,8 @@ static int ec_coe_download_segment(ec_slave_t *slave,
     return 0;
 }
 
-int ec_coe_download(ec_slave_t *slave,
+int ec_coe_download(ec_master_t *master,
+                    uint16_t slave_index,
                     ec_datagram_t *datagram,
                     uint16_t index,
                     uint8_t subindex,
@@ -266,22 +266,29 @@ int ec_coe_download(ec_slave_t *slave,
                     uint32_t size,
                     bool complete_access)
 {
+    ec_slave_t *slave;
     uint8_t *ptr;
     uint32_t seg_size, max_data_size;
     bool toggle;
     bool last;
     int ret;
 
+    if (slave_index >= master->slave_count) {
+        return -EC_ERR_INVAL;
+    }
+
+    slave = &master->slaves[slave_index];
+
     ptr = (uint8_t *)buf;
 
     if (size <= 4) {
-        return ec_coe_download_expedited(slave, datagram, index, subindex, ptr, size, complete_access);
+        return ec_coe_download_expedited(master, slave_index, datagram, index, subindex, ptr, size, complete_access);
     } else {
         max_data_size = slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_COE_DOWN_REQ_HEADER_SIZE;
         if (size <= max_data_size) {
-            return ec_coe_download_common(slave, datagram, index, subindex, ptr, size, complete_access);
+            return ec_coe_download_common(master, slave_index, datagram, index, subindex, ptr, size, complete_access);
         } else {
-            ret = ec_coe_download_common(slave, datagram, index, subindex, ptr, size, complete_access);
+            ret = ec_coe_download_common(master, slave_index, datagram, index, subindex, ptr, max_data_size, complete_access);
             if (ret < 0) {
                 return ret;
             }
@@ -294,10 +301,10 @@ int ec_coe_download(ec_slave_t *slave,
 
             while (1) {
                 seg_size = MIN(size, max_data_size);
-                if (seg_size < max_data_size) {
+                if (size <= max_data_size) {
                     last = true;
                 }
-                ret = ec_coe_download_segment(slave, datagram, ptr, seg_size, toggle, last);
+                ret = ec_coe_download_segment(master, slave_index, datagram, ptr, seg_size, toggle, last);
                 if (ret < 0) {
                     return ret;
                 }
@@ -312,7 +319,8 @@ int ec_coe_download(ec_slave_t *slave,
     }
 }
 
-int ec_coe_upload(ec_slave_t *slave,
+int ec_coe_upload(ec_master_t *master,
+                  uint16_t slave_index,
                   ec_datagram_t *datagram,
                   uint16_t index,
                   uint8_t subindex,
@@ -337,7 +345,7 @@ int ec_coe_upload(ec_slave_t *slave,
 
     ptr = (uint8_t *)buf;
 
-    data = ec_mailbox_fill_send(slave, datagram, EC_MBOX_TYPE_COE, EC_COE_UP_REQ_HEADER_SIZE);
+    data = ec_mailbox_fill_send(master, slave_index, datagram, EC_MBOX_TYPE_COE, EC_COE_UP_REQ_HEADER_SIZE);
 
     upload_common = (ec_coe_upload_common_header_t *)data;
     upload_common->coe_header.number = 0;
@@ -353,12 +361,12 @@ int ec_coe_upload(ec_slave_t *slave,
     upload_common->subindex = complete_access ? 0x00 : subindex;
 
     memset(upload_common->data, 0x00, 4);
-    ret = ec_mailbox_send(slave, datagram);
+    ret = ec_mailbox_send(master, slave_index, datagram);
     if (ret < 0) {
         return ret;
     }
 
-    ret = ec_mailbox_receive(slave, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
+    ret = ec_mailbox_receive(master, slave_index, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
     if (ret < 0) {
         return ret;
     }
@@ -375,7 +383,7 @@ int ec_coe_upload(ec_slave_t *slave,
 
     if (EC_READ_U16(data) >> 12 == EC_COE_SERVICE_SDO_REQUEST &&
         EC_READ_U8(data + 2) >> 5 == EC_COE_REQUEST_ABORT) {
-        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave->index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
+        EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave_index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
         return -EC_ERR_COE_ABORT;
     }
 
@@ -437,7 +445,7 @@ int ec_coe_upload(ec_slave_t *slave,
 
         if (offset < total_size) {
             while (1) {
-                data = ec_mailbox_fill_send(slave, datagram, EC_MBOX_TYPE_COE, EC_COE_UP_REQ_HEADER_SIZE);
+                data = ec_mailbox_fill_send(master, slave_index, datagram, EC_MBOX_TYPE_COE, EC_COE_UP_REQ_HEADER_SIZE);
 
                 upload_seg = (ec_coe_upload_segment_header_t *)data;
                 upload_seg->coe_header.number = 0;
@@ -448,12 +456,12 @@ int ec_coe_upload(ec_slave_t *slave,
                 upload_seg->sdo_header.command = EC_COE_REQUEST_SEGMENT_UPLOAD;
                 upload_seg->sdo_header.segdata_size = 0;
                 memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE, 0x00, 7);
-                ret = ec_mailbox_send(slave, datagram);
+                ret = ec_mailbox_send(master, slave_index, datagram);
                 if (ret < 0) {
                     return ret;
                 }
 
-                ret = ec_mailbox_receive(slave, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
+                ret = ec_mailbox_receive(master, slave_index, datagram, &mbox_proto, &recv_size, EC_COE_TIMEOUT_US);
                 if (ret < 0) {
                     return ret;
                 }
@@ -470,7 +478,7 @@ int ec_coe_upload(ec_slave_t *slave,
 
                 if (EC_READ_U16(data) >> 12 == EC_COE_SERVICE_SDO_REQUEST &&
                     EC_READ_U8(data + 2) >> 5 == EC_COE_REQUEST_ABORT) {
-                    EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave->index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
+                    EC_SLAVE_LOG_ERR("Slave %u SDO abort code: 0x%08x (%s)\n", slave_index, EC_READ_U32(data + 6), ec_sdo_abort_string(EC_READ_U32(data + 6)));
                     return -EC_ERR_COE_ABORT;
                 }
 

src/ec_mailbox.c

@@ -7,8 +7,17 @@
 
 #define EC_MBOX_READ_TIMEOUT (100 * 1000)
 
-uint8_t *ec_mailbox_fill_send(ec_slave_t *slave, ec_datagram_t *datagram, uint8_t type, uint16_t size)
+uint8_t *ec_mailbox_fill_send(ec_master_t *master,
+                              uint16_t slave_index,
+                              ec_datagram_t *datagram,
+                              uint8_t type,
+                              uint16_t size)
 {
+    ec_slave_t *slave;
+
+    slave = &master->slaves[slave_index];
+
+    EC_ASSERT_MSG(slave_index >= master->slave_count, "Invalid slave index");
     EC_ASSERT_MSG((EC_MBOX_HEADER_SIZE + size) <= slave->configured_rx_mailbox_size, "RX Mailbox size overflow");
 
     EC_WRITE_U16(datagram->data, size);                       // mailbox service data length
@@ -19,18 +28,38 @@ uint8_t *ec_mailbox_fill_send(ec_slave_t *slave, ec_datagram_t *datagram, uint8_
     return (datagram->data + EC_MBOX_HEADER_SIZE);
 }
 
-int ec_mailbox_send(ec_slave_t *slave, ec_datagram_t *datagram)
+int ec_mailbox_send(ec_master_t *master,
+                    uint16_t slave_index,
+                    ec_datagram_t *datagram)
 {
+    ec_slave_t *slave;
+
+    if (slave_index >= master->slave_count) {
+        return -EC_ERR_INVAL;
+    }
+
+    slave = &master->slaves[slave_index];
+
     ec_datagram_fpwr(datagram, slave->station_address, slave->configured_rx_mailbox_offset, slave->configured_rx_mailbox_size);
     datagram->netdev_idx = slave->netdev_idx;
     return ec_master_queue_ext_datagram(slave->master, datagram, true, true);
 }
 
-int ec_mailbox_read_status(ec_slave_t *slave, ec_datagram_t *datagram, uint32_t timeout_us)
+int ec_mailbox_read_status(ec_master_t *master,
+                           uint16_t slave_index,
+                           ec_datagram_t *datagram,
+                           uint32_t timeout_us)
 {
+    ec_slave_t *slave;
     uint32_t start_time;
     int ret;
 
+    if (slave_index >= master->slave_count) {
+        return -EC_ERR_INVAL;
+    }
+
+    slave = &master->slaves[slave_index];
+
     start_time = jiffies;
 
 check_again:
@@ -52,14 +81,26 @@ check_again:
     return 0;
 }
 
-int ec_mailbox_receive(ec_slave_t *slave, ec_datagram_t *datagram, uint8_t *type, uint32_t *size, uint32_t timeout_us)
+int ec_mailbox_receive(ec_master_t *master,
+                       uint16_t slave_index,
+                       ec_datagram_t *datagram,
+                       uint8_t *type,
+                       uint32_t *size,
+                       uint32_t timeout_us)
 {
+    ec_slave_t *slave;
     uint16_t code;
     uint32_t tmp_size;
     uint8_t tmp_type;
     int ret;
 
-    ret = ec_mailbox_read_status(slave, datagram, timeout_us);
+    if (slave_index >= master->slave_count) {
+        return -EC_ERR_INVAL;
+    }
+
+    slave = &master->slaves[slave_index];
+
+    ret = ec_mailbox_read_status(master, slave_index, datagram, timeout_us);
     if (ret < 0) {
         return ret;
     }

src/ec_sii.c

@@ -115,14 +115,21 @@ sii_check:
     return 0;
 }
 
-int ec_sii_read(ec_slave_t *slave, ec_datagram_t *datagram, uint16_t woffset, uint32_t *buf, uint32_t len)
+int ec_sii_read(ec_master_t *master, uint16_t slave_index, ec_datagram_t *datagram, uint16_t woffset, uint32_t *buf, uint32_t len)
 {
+    ec_slave_t *slave;
     int ret;
 
     if (len % 4) {
         return -EC_ERR_INVAL;
     }
 
+    if (slave_index >= master->slave_count) {
+        return -EC_ERR_INVAL;
+    }
+
+    slave = &master->slaves[slave_index];
+
     ret = ec_sii_assign_master(slave, datagram);
     if (ret < 0) {
         return ret;
@@ -138,14 +145,21 @@ int ec_sii_read(ec_slave_t *slave, ec_datagram_t *datagram, uint16_t woffset, ui
     return esc_sii_assign_pdi(slave, datagram);
 }
 
-int ec_sii_write(ec_slave_t *slave, ec_datagram_t *datagram, uint16_t woffset, const uint16_t *buf, uint32_t len)
+int ec_sii_write(ec_master_t *master, uint16_t slave_index, ec_datagram_t *datagram, uint16_t woffset, const uint16_t *buf, uint32_t len)
 {
+    ec_slave_t *slave;
     int ret;
 
     if (len % 2) {
         return -EC_ERR_INVAL;
     }
 
+    if (slave_index >= master->slave_count) {
+        return -EC_ERR_INVAL;
+    }
+
+    slave = &master->slaves[slave_index];
+
     ret = ec_sii_assign_master(slave, datagram);
     if (ret < 0) {
         return ret;

src/ec_slave.c

@@ -14,11 +14,11 @@
 
 /** Maximum count to wait for clock discipline.
  */
-#define EC_DC_SYNC_WATI_COUNT  (15000)
+#define EC_DC_SYNC_WATI_COUNT (15000)
 
 /** Time offset (in ns), that is added to cyclic start time.
  */
-#define EC_DC_START_OFFSET     100000000ULL
+#define EC_DC_START_OFFSET 100000000ULL
 
 static void ec_slave_init(ec_slave_t *slave,
                           uint32_t slave_index,
@@ -270,21 +270,21 @@ void ec_slave_calc_transmission_delays(ec_slave_t *slave, uint32_t *delay)
     *delay = *delay + slave->ports[0].delay_to_next_dc;
 }
 
-static int ec_slave_state_clear_ack_error(ec_master_t *master, ec_slave_t *slave, ec_slave_state_t requested_state)
+static int ec_slave_state_clear_ack_error(ec_slave_t *slave, ec_slave_state_t requested_state)
 {
     ec_datagram_t *datagram;
     int ret;
     uint64_t start_time;
     uint32_t status_code;
 
-    datagram = &master->main_datagram;
+    datagram = &slave->master->main_datagram;
 
     start_time = jiffies;
 
     ec_datagram_fprd(datagram, slave->station_address, ESCREG_OF(ESCREG->AL_STAT_CODE), 2);
     ec_datagram_zero(datagram);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -296,7 +296,7 @@ static int ec_slave_state_clear_ack_error(ec_master_t *master, ec_slave_t *slave
     ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->AL_CTRL), 2);
     EC_WRITE_U16(datagram->data, slave->current_state);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -305,7 +305,7 @@ repeat_check:
     ec_datagram_fprd(datagram, slave->station_address, ESCREG_OF(ESCREG->AL_STAT), 2);
     ec_datagram_zero(datagram);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -341,14 +341,14 @@ repeat_check:
  *    and write AL control register to acknowledge error, then repeat 2
  * 5. if state is changed to other state, repeat step 2
 */
-static int ec_slave_state_change(ec_master_t *master, ec_slave_t *slave, ec_slave_state_t requested_state)
+static int ec_slave_state_change(ec_slave_t *slave, ec_slave_state_t requested_state)
 {
     ec_datagram_t *datagram;
     int ret;
     uint32_t start_time;
     ec_slave_state_t old_state;
 
-    datagram = &master->main_datagram;
+    datagram = &slave->master->main_datagram;
 
     old_state = slave->current_state;
     start_time = jiffies;
@@ -356,7 +356,7 @@ static int ec_slave_state_change(ec_master_t *master, ec_slave_t *slave, ec_slav
     ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->AL_CTRL), 2);
     EC_WRITE_U16(datagram->data, requested_state);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -365,7 +365,7 @@ repeat_check:
     ec_datagram_fprd(datagram, slave->station_address, ESCREG_OF(ESCREG->AL_STAT), 2);
     ec_datagram_zero(datagram);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -379,7 +379,7 @@ repeat_check:
 
     if (slave->current_state != old_state) {
         if ((slave->current_state & 0x0F) == (old_state & 0x0F)) { // acknowledge bit enable
-            return ec_slave_state_clear_ack_error(master, slave, requested_state);
+            return ec_slave_state_clear_ack_error(slave, requested_state);
         } else {
             old_state = slave->current_state;
             if ((jiffies - start_time) > ec_slave_state_change_timeout_us(slave->current_state, requested_state)) {
@@ -465,15 +465,15 @@ static int ec_slave_config_dc_systime_and_delay(ec_slave_t *slave)
     return 0;
 }
 
-static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
+static int ec_slave_config(ec_slave_t *slave)
 {
     ec_datagram_t *datagram;
     uint64_t start_time;
     int ret;
 
-    datagram = &master->main_datagram;
+    datagram = &slave->master->main_datagram;
 
-    ret = ec_slave_state_change(master, slave, EC_SLAVE_STATE_INIT);
+    ret = ec_slave_state_change(slave, EC_SLAVE_STATE_INIT);
     if (ret < 0) {
         return ret;
     }
@@ -482,7 +482,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
     ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->FMMU[0]), EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);
     ec_datagram_zero(datagram);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -491,7 +491,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
     ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->SYNCM[0]), EC_SYNC_PAGE_SIZE * slave->base_sync_count);
     ec_datagram_zero(datagram);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -500,7 +500,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
     ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->CYC_UNIT_CTRL), 2);
     ec_datagram_zero(datagram);
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -530,7 +530,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
             ec_slave_sm_config(&sm_info[i], datagram->data + EC_SYNC_PAGE_SIZE * i);
         }
         datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
         if (ret < 0) {
             return ret;
         }
@@ -540,7 +540,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         slave->configured_tx_mailbox_offset = slave->sii.boot_tx_mailbox_offset;
         slave->configured_tx_mailbox_size = slave->sii.boot_tx_mailbox_size;
 
-        ret = ec_slave_state_change(master, slave, EC_SLAVE_STATE_BOOT);
+        ret = ec_slave_state_change(slave, EC_SLAVE_STATE_BOOT);
         if (ret < 0) {
             EC_SLAVE_LOG_ERR("Failed to change state to %s on slave %u\n",
                              ec_state_string(slave->requested_state, 0), slave->index);
@@ -557,7 +557,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         ec_slave_sm_config(&slave->sm_info[i], datagram->data + EC_SYNC_PAGE_SIZE * i);
     }
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -567,7 +567,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
     slave->configured_tx_mailbox_offset = slave->sm_info[EC_SM_INDEX_MBX_READ].physical_start_address;
     slave->configured_tx_mailbox_size = slave->sm_info[EC_SM_INDEX_MBX_READ].length;
 
-    ret = ec_slave_state_change(master, slave, EC_SLAVE_STATE_PREOP);
+    ret = ec_slave_state_change(slave, EC_SLAVE_STATE_PREOP);
     if (ret < 0) {
         EC_SLAVE_LOG_ERR("Failed to change state to %s on slave %u\n",
                          ec_state_string(slave->requested_state, 0), slave->index);
@@ -584,35 +584,35 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
          * Set number of assigned entries
         */
         data = 0;
-        ret = ec_coe_download(slave, datagram, 0x1c12, 0x00, &data, 2, false);
+        ret = ec_coe_download(slave->master, slave->index, datagram, 0x1c12, 0x00, &data, 2, false);
         if (ret < 0) {
             return ret;
         }
-        ret = ec_coe_download(slave, datagram, 0x1c13, 0x00, &data, 2, false);
+        ret = ec_coe_download(slave->master, slave->index, datagram, 0x1c13, 0x00, &data, 2, false);
         if (ret < 0) {
             return ret;
         }
         for (uint32_t i = 0; i < slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.count; i++) {
             data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i];
-            ret = ec_coe_download(slave, datagram, 0x1c12, 0x01 + i, &data, 2, false);
+            ret = ec_coe_download(slave->master, slave->index, datagram, 0x1c12, 0x01 + i, &data, 2, false);
             if (ret < 0) {
                 return ret;
             }
         }
         for (uint32_t i = 0; i < slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.count; i++) {
             data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i];
-            ret = ec_coe_download(slave, datagram, 0x1c13, 0x01 + i, &data, 2, false);
+            ret = ec_coe_download(slave->master, slave->index, datagram, 0x1c13, 0x01 + i, &data, 2, false);
             if (ret < 0) {
                 return ret;
             }
         }
         data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.count;
-        ret = ec_coe_download(slave, datagram, 0x1c12, 0x00, &data, 2, false);
+        ret = ec_coe_download(slave->master, slave->index, datagram, 0x1c12, 0x00, &data, 2, false);
         if (ret < 0) {
             return ret;
         }
         data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.count;
-        ret = ec_coe_download(slave, datagram, 0x1c13, 0x00, &data, 2, false);
+        ret = ec_coe_download(slave->master, slave->index, datagram, 0x1c13, 0x00, &data, 2, false);
         if (ret < 0) {
             return ret;
         }
@@ -626,21 +626,21 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         if (slave->sii.general.coe_details.enable_pdo_configuration) {
             for (uint32_t i = 0; i < slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.count; i++) {
                 data = 0;
-                ret = ec_coe_download(slave, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
+                ret = ec_coe_download(slave->master, slave->index, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
                 if (ret < 0) {
                     return ret;
                 }
 
                 for (uint32_t j = 0; j < slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_mapping[i].count; j++) {
                     data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_mapping[i].entry[j];
-                    ret = ec_coe_download(slave, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i], 0x01 + j, &data, 4, false);
+                    ret = ec_coe_download(slave->master, slave->index, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i], 0x01 + j, &data, 4, false);
                     if (ret < 0) {
                         return ret;
                     }
                 }
 
                 data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_mapping[i].count;
-                ret = ec_coe_download(slave, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
+                ret = ec_coe_download(slave->master, slave->index, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
                 if (ret < 0) {
                     return ret;
                 }
@@ -648,21 +648,21 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
 
             for (uint32_t i = 0; i < slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.count; i++) {
                 data = 0;
-                ret = ec_coe_download(slave, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
+                ret = ec_coe_download(slave->master, slave->index, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
                 if (ret < 0) {
                     return ret;
                 }
 
                 for (uint32_t j = 0; j < slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_mapping[i].count; j++) {
                     data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_mapping[i].entry[j];
-                    ret = ec_coe_download(slave, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i], 0x01 + j, &data, 4, false);
+                    ret = ec_coe_download(slave->master, slave->index, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i], 0x01 + j, &data, 4, false);
                     if (ret < 0) {
                         return ret;
                     }
                 }
 
                 data = slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_mapping[i].count;
-                ret = ec_coe_download(slave, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
+                ret = ec_coe_download(slave->master, slave->index, datagram, slave->sm_info[EC_SM_INDEX_PROCESS_DATA_INPUT].pdo_assign.entry[i], 0x00, &data, 1, false);
                 if (ret < 0) {
                     return ret;
                 }
@@ -682,7 +682,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         ec_slave_sm_config(&slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT + i], datagram->data + EC_SYNC_PAGE_SIZE * i);
     }
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -693,7 +693,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         ec_slave_fmmu_config(&slave->sm_info[EC_SM_INDEX_PROCESS_DATA_OUTPUT + i], datagram->data + EC_FMMU_PAGE_SIZE * i);
     }
     datagram->netdev_idx = slave->netdev_idx;
-    ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+    ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
     if (ret < 0) {
         return ret;
     }
@@ -710,7 +710,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         EC_WRITE_U32(datagram->data, slave->config->dc_sync[0].cycle_time);
         EC_WRITE_U32(datagram->data + 4, slave->config->dc_sync[1].cycle_time);
         datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
         if (ret < 0) {
             return ret;
         }
@@ -720,7 +720,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         ec_datagram_fprd(datagram, slave->station_address, ESCREG_OF(ESCREG->SYS_TIME_DIFF), 4);
         ec_datagram_zero(datagram);
         datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
         if (ret < 0) {
             return ret;
         }
@@ -748,7 +748,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
 
         EC_WRITE_U64(datagram->data, dc_start_time);
         datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
         if (ret < 0) {
             return ret;
         }
@@ -756,13 +756,13 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->CYC_UNIT_CTRL), 2);
         EC_WRITE_U16(datagram->data, slave->config->dc_assign_activate);
         datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
         if (ret < 0) {
             return ret;
         }
     }
 
-    ret = ec_slave_state_change(master, slave, EC_SLAVE_STATE_SAFEOP);
+    ret = ec_slave_state_change(slave, EC_SLAVE_STATE_SAFEOP);
     if (ret < 0) {
         EC_SLAVE_LOG_ERR("Failed to change state to %s on slave %u\n",
                          ec_state_string(slave->requested_state, 0), slave->index);
@@ -774,7 +774,7 @@ static int ec_slave_config(ec_master_t *master, ec_slave_t *slave)
         return 0;
     }
 
-    ret = ec_slave_state_change(master, slave, EC_SLAVE_STATE_OP);
+    ret = ec_slave_state_change(slave, EC_SLAVE_STATE_OP);
     if (ret < 0) {
         EC_SLAVE_LOG_ERR("Failed to change state to %s on slave %u\n",
                          ec_state_string(slave->requested_state, 0), slave->index);
@@ -930,7 +930,7 @@ static void ec_master_scan_slaves_state(ec_master_t *master)
         }
 
         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
-            ret = ec_slave_state_clear_ack_error(master, slave, slave->requested_state);
+            ret = ec_slave_state_clear_ack_error(slave, slave->requested_state);
             if (ret < 0) {
                 continue;
             }
@@ -939,10 +939,9 @@ static void ec_master_scan_slaves_state(ec_master_t *master)
         }
 
         if (((slave->requested_state != slave->current_state) && (slave->alstatus_code == 0)) || slave->force_update) {
-            ret = ec_slave_config(master, slave);
+            ret = ec_slave_config(slave);
             if (ret < 0) {
                 EC_SLAVE_LOG_ERR("Failed to configure slave %u\n", slave->index);
-                continue;
             }
             slave->force_update = false;
         }
@@ -970,6 +969,7 @@ void ec_slaves_scanning(ec_master_t *master)
             EC_LOG_INFO("Detect link down on %s\n",
                         master->netdev[netdev_idx]->name);
 
+            ec_osal_mutex_take(master->scan_lock);
             ec_master_clear_slaves(master);
 
             for (uint8_t i = EC_NETDEV_MAIN; i < CONFIG_EC_MAX_NETDEVS; i++) {
@@ -977,6 +977,7 @@ void ec_slaves_scanning(ec_master_t *master)
                 master->slaves_responding[i] = 0;
             }
             master->scan_done = false;
+            ec_osal_mutex_give(master->scan_lock);
         }
         master->link_state[netdev_idx] = master->netdev[netdev_idx]->link_state;
     }
@@ -1016,6 +1017,8 @@ void ec_slaves_scanning(ec_master_t *master)
 
         rescan_required = 0;
 
+        ec_osal_mutex_take(master->scan_lock);
+
         master->scan_done = false;
         EC_LOG_INFO("Rescanning bus...\n");
 
@@ -1028,13 +1031,13 @@ void ec_slaves_scanning(ec_master_t *master)
         }
 
         if (!count) {
-            return;
+            goto mutex_unlock;
         }
 
         master->slaves = ec_osal_malloc(sizeof(ec_slave_t) * count);
         if (!master->slaves) {
             EC_LOG_ERR("Failed to allocate memory for slaves\n");
-            return;
+            goto mutex_unlock;
         }
 
         master->slave_count = count;
@@ -1052,6 +1055,7 @@ void ec_slaves_scanning(ec_master_t *master)
                 autoinc_address++;
             }
         }
+        ec_osal_mutex_give(master->scan_lock);
 
         for (uint8_t netdev_idx = EC_NETDEV_MAIN; netdev_idx < CONFIG_EC_MAX_NETDEVS; netdev_idx++) {
             if (master->slaves_responding[netdev_idx] == 0) {
@@ -1064,7 +1068,7 @@ void ec_slaves_scanning(ec_master_t *master)
             ret = ec_master_queue_ext_datagram(master, datagram, true, true);
             if (ret < 0) {
                 EC_LOG_ERR("Failed to clear station address on %s link\n", master->netdev[netdev_idx]->name);
-                return;
+                goto mutex_unlock;
             }
 
             // Clear recevice time for dc measure delays
@@ -1074,7 +1078,7 @@ void ec_slaves_scanning(ec_master_t *master)
             ret = ec_master_queue_ext_datagram(master, datagram, true, true);
             if (ret < 0) {
                 EC_LOG_ERR("Failed to clear receive time on %s link\n", master->netdev[netdev_idx]->name);
-                return;
+                goto mutex_unlock;
             }
         }
 
@@ -1090,7 +1094,7 @@ void ec_slaves_scanning(ec_master_t *master)
             ret = ec_master_queue_ext_datagram(master, datagram, true, true);
             if (ret < 0) {
                 EC_LOG_ERR("Failed to set station address on slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
 
             // Read AL state
@@ -1100,7 +1104,7 @@ void ec_slaves_scanning(ec_master_t *master)
             ret = ec_master_queue_ext_datagram(master, datagram, true, true);
             if (ret < 0) {
                 EC_LOG_ERR("Failed to read AL status on slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
 
             // Read base information
@@ -1110,7 +1114,7 @@ void ec_slaves_scanning(ec_master_t *master)
             ret = ec_master_queue_ext_datagram(master, datagram, true, true);
             if (ret < 0) {
                 EC_SLAVE_LOG_ERR("Failed to read base on slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
 
             slave->base_type = EC_READ_U8(datagram->data);
@@ -1150,7 +1154,7 @@ void ec_slaves_scanning(ec_master_t *master)
                 ret = ec_master_queue_ext_datagram(master, datagram, true, true);
                 if (ret < 0) {
                     EC_SLAVE_LOG_ERR("Failed to read DC capabilities on slave %u\n", slave->index);
-                    return;
+                    goto mutex_unlock;
                 }
 
                 if (datagram->working_counter == 1) {
@@ -1168,7 +1172,7 @@ void ec_slaves_scanning(ec_master_t *master)
                 ret = ec_master_queue_ext_datagram(master, datagram, true, true);
                 if (ret < 0) {
                     EC_SLAVE_LOG_ERR("Failed to read DC receive times on slave %u\n", slave->index);
-                    return;
+                    goto mutex_unlock;
                 }
 
                 for (uint8_t i = 0; i < EC_MAX_PORTS; i++) {
@@ -1184,7 +1188,7 @@ void ec_slaves_scanning(ec_master_t *master)
             ret = ec_master_queue_ext_datagram(master, datagram, true, true);
             if (ret < 0) {
                 EC_SLAVE_LOG_ERR("Failed to read data link status on slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
 
             uint16_t dl_status = EC_READ_U16(datagram->data);
@@ -1204,10 +1208,10 @@ void ec_slaves_scanning(ec_master_t *master)
 
             // Read SII category headers to determine full SII size
             do {
-                ret = ec_sii_read(slave, datagram, sii_offset, &sii_data, 4);
+                ret = ec_sii_read(master, slave_index, datagram, sii_offset, &sii_data, 4);
                 if (ret < 0) {
                     EC_SLAVE_LOG_ERR("Failed to read SII category header on slave %u\n", slave->index);
-                    return;
+                    goto mutex_unlock;
                 }
 
                 cat_type = sii_data & 0xFFFF;
@@ -1223,15 +1227,15 @@ void ec_slaves_scanning(ec_master_t *master)
             slave->sii_image = ec_osal_malloc(slave->sii_nwords * 2);
             if (!slave->sii_image) {
                 EC_LOG_ERR("Failed to allocate memory for SII on slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
             memset(slave->sii_image, 0, slave->sii_nwords * 2);
 
             // Read full SII and parse it
-            ret = ec_sii_read(slave, datagram, 0x0000, (uint32_t *)slave->sii_image, slave->sii_nwords * 2);
+            ret = ec_sii_read(master, slave_index, datagram, 0x0000, (uint32_t *)slave->sii_image, slave->sii_nwords * 2);
             if (ret < 0) {
                 EC_SLAVE_LOG_ERR("Failed to read SII category header on slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
 
             slave->sii.aliasaddr =
@@ -1282,7 +1286,7 @@ void ec_slaves_scanning(ec_master_t *master)
                         ret = ec_slave_fetch_sii_strings(slave, (uint8_t *)cat_data, cat_size * 2);
                         if (ret < 0) {
                             EC_SLAVE_LOG_ERR("Failed to fetch SII strings on slave %u\n", slave->index);
-                            return;
+                            goto mutex_unlock;
                         }
                         break;
                     case EC_SII_TYPE_GENERAL:
@@ -1321,10 +1325,10 @@ void ec_slaves_scanning(ec_master_t *master)
 
             EC_SLAVE_LOG_INFO("Slave %u parse eeprom success\n", slave->index);
 
-            ret = ec_slave_config(master, slave);
+            ret = ec_slave_config(slave);
             if (ret < 0) {
                 EC_SLAVE_LOG_ERR("Failed to configure slave %u\n", slave->index);
-                return;
+                goto mutex_unlock;
             }
         }
 
@@ -1332,6 +1336,9 @@ void ec_slaves_scanning(ec_master_t *master)
         master->scan_done = true;
 
         ec_master_calc_dc(master);
+
+    mutex_unlock:
+        ec_osal_mutex_give(master->scan_lock);
     }
 
     if (master->slave_count && master->scan_done) {