diff --git a/include/ecrt.h b/include/ecrt.h
index fa1b3498c6e1032569e8c5a13d83e72b7fd88b3a..d3b8273158e1abfda22165695b9c0854907a1535 100644
--- a/include/ecrt.h
+++ b/include/ecrt.h
@@ -687,6 +687,7 @@ int ecrt_master_sdo_upload(
int ecrt_master_write_idn(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
+ uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
uint8_t *data, /**< Pointer to data to write. */
size_t data_size, /**< Size of data to write. */
@@ -705,6 +706,7 @@ int ecrt_master_write_idn(
int ecrt_master_read_idn(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
+ uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
uint8_t *target, /**< Pointer to memory where the read data can be
stored. */
@@ -1212,6 +1214,7 @@ void ecrt_slave_config_state(
*/
int ecrt_slave_config_idn(
ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< SoE IDN. */
ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or
SAFEOP). */
diff --git a/lib/master.c b/lib/master.c
index 0c9d7dbbd1a58bf661e5db39bebfc962768d81c5..382cd1005c07b2c2cadfd2c520a99a6c3b66e80d 100644
--- a/lib/master.c
+++ b/lib/master.c
@@ -305,11 +305,13 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
/*****************************************************************************/
int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
- uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
+ uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
+ uint16_t *error_code)
{
ec_ioctl_slave_soe_write_t io;
io.slave_position = slave_position;
+ io.drive_no = drive_no;
io.idn = idn;
io.data_size = data_size;
io.data = data;
@@ -328,12 +330,13 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
/*****************************************************************************/
int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
- uint16_t idn, uint8_t *target, size_t target_size,
+ uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
size_t *result_size, uint16_t *error_code)
{
ec_ioctl_slave_soe_read_t io;
io.slave_position = slave_position;
+ io.drive_no = drive_no;
io.idn = idn;
io.mem_size = target_size;
io.data = target;
diff --git a/lib/slave_config.c b/lib/slave_config.c
index 49ce635b4fcb85bc0bb17e6985878bad3c2310d3..97b3526e1749abcde8f8cc353b044ac7ce9de1d4 100644
--- a/lib/slave_config.c
+++ b/lib/slave_config.c
@@ -472,12 +472,13 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
/*****************************************************************************/
-int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
- ec_al_state_t al_state, const uint8_t *data, size_t size)
+int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
+ uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size)
{
ec_ioctl_sc_idn_t io;
io.config_index = sc->index;
+ io.drive_no = drive_no;
io.idn = idn;
io.al_state = al_state;
io.data = data;
diff --git a/master/cdev.c b/master/cdev.c
index 1ae289cf409b3751c97106fb843f78a48b3464b8..33c4b5721b1a1a6c4221b75b24a9916d45d31b54 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -2474,7 +2474,7 @@ int ec_cdev_ioctl_sc_idn(
up(&master->master_sem); // FIXME
ret = ecrt_slave_config_idn(
- sc, ioctl.idn, ioctl.al_state, data, ioctl.size);
+ sc, ioctl.drive_no, ioctl.idn, ioctl.al_state, data, ioctl.size);
kfree(data);
return ret;
}
@@ -3344,7 +3344,7 @@ int ec_cdev_ioctl_slave_soe_read(
}
retval = ecrt_master_read_idn(master, ioctl.slave_position,
- ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
+ ioctl.drive_no, ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
&ioctl.error_code);
if (retval) {
kfree(data);
@@ -3395,7 +3395,8 @@ int ec_cdev_ioctl_slave_soe_write(
}
retval = ecrt_master_write_idn(master, ioctl.slave_position,
- ioctl.idn, data, ioctl.data_size, &ioctl.error_code);
+ ioctl.drive_no, ioctl.idn, data, ioctl.data_size,
+ &ioctl.error_code);
kfree(data);
if (retval) {
return retval;
diff --git a/master/fsm_soe.c b/master/fsm_soe.c
index a43ba1178cf4c1b9963dac0a00a5510d1a9d15ac..7535c10e19473b1dfe7022c4a112e9e70a4d5c87 100644
--- a/master/fsm_soe.c
+++ b/master/fsm_soe.c
@@ -201,7 +201,8 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
ec_soe_request_t *request = fsm->request;
uint8_t *data;
- EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X.\n", request->idn);
+ EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
+ request->drive_no);
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
@@ -218,7 +219,7 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
return;
}
- EC_WRITE_U8(data, OPCODE_READ_REQUEST);
+ EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
EC_WRITE_U8(data + 1, 1 << 6); // request value
EC_WRITE_U16(data + 2, request->idn);
@@ -492,7 +493,8 @@ void ec_fsm_soe_write_next_fragment(
return;
}
- EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3);
+ EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |
+ (req->drive_no & 0x07) << 5);
EC_WRITE_U8(data + 1, 1 << 6); // only value included
EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
memcpy(data + 4, req->data + fsm->offset, fragment_size);
@@ -517,8 +519,8 @@ void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *req = fsm->request;
- EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X (%zu byte).\n",
- req->idn, req->data_size);
+ EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n",
+ req->idn, req->drive_no, req->data_size);
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
diff --git a/master/ioctl.h b/master/ioctl.h
index 2606822a038b95383cd8ae86664928aa3ddee0f6..e9337d5c373e763708925bbadbf8a6a94ab54579 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -56,7 +56,7 @@
*
* Increment this when changing the ioctl interface!
*/
-#define EC_IOCTL_VERSION_MAGIC 6
+#define EC_IOCTL_VERSION_MAGIC 7
// Command-line tool
#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t)
@@ -413,6 +413,7 @@ typedef struct {
typedef struct {
// inputs
uint16_t slave_position;
+ uint8_t drive_no;
uint16_t idn;
uint32_t mem_size;
uint8_t *data;
@@ -427,6 +428,7 @@ typedef struct {
typedef struct {
// inputs
uint16_t slave_position;
+ uint8_t drive_no;
uint16_t idn;
uint32_t data_size;
uint8_t *data;
@@ -582,6 +584,7 @@ typedef struct {
typedef struct {
// inputs
uint32_t config_index;
+ uint8_t drive_no;
uint16_t idn;
ec_al_state_t al_state;
const uint8_t *data;
diff --git a/master/master.c b/master/master.c
index 54e600d7a223e55d7d76b246ec1b5d055842671f..0bbe5b76d0baca1ca8cb851ae391d9cd6d34f111 100644
--- a/master/master.c
+++ b/master/master.c
@@ -2345,13 +2345,20 @@ uint32_t ecrt_master_sync_monitor_process(ec_master_t *master)
/*****************************************************************************/
int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
- uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
+ uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
+ uint16_t *error_code)
{
ec_master_soe_request_t request;
int retval;
+ if (drive_no > 7) {
+ EC_MASTER_ERR(master, "Invalid drive number!\n");
+ return -EINVAL;
+ }
+
INIT_LIST_HEAD(&request.list);
ec_soe_request_init(&request.req);
+ ec_soe_request_set_drive_no(&request.req, drive_no);
ec_soe_request_set_idn(&request.req, idn);
if (ec_soe_request_alloc(&request.req, data_size)) {
@@ -2413,13 +2420,19 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
/*****************************************************************************/
int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
- uint16_t idn, uint8_t *target, size_t target_size,
+ uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
size_t *result_size, uint16_t *error_code)
{
ec_master_soe_request_t request;
+ if (drive_no > 7) {
+ EC_MASTER_ERR(master, "Invalid drive number!\n");
+ return -EINVAL;
+ }
+
INIT_LIST_HEAD(&request.list);
ec_soe_request_init(&request.req);
+ ec_soe_request_set_drive_no(&request.req, drive_no);
ec_soe_request_set_idn(&request.req, idn);
ec_soe_request_read(&request.req);
diff --git a/master/slave_config.c b/master/slave_config.c
index f99667d8d3376d2c805b2b3c0e7b73da5ed59d5f..1ff03122860decfde61a64dfab261beaf24f95e8 100644
--- a/master/slave_config.c
+++ b/master/slave_config.c
@@ -938,16 +938,22 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
/*****************************************************************************/
-int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
- ec_al_state_t state, const uint8_t *data, size_t size)
+int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
+ uint16_t idn, ec_al_state_t state, const uint8_t *data,
+ size_t size)
{
ec_slave_t *slave = sc->slave;
ec_soe_request_t *req;
int ret;
- EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, state = %u, "
- "data = 0x%p, size = %zu)\n",
- __func__, sc, idn, state, data, size);
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, drive_no = %u, idn = 0x%04X, "
+ "state = %u, data = 0x%p, size = %zu)\n",
+ __func__, sc, drive_no, idn, state, data, size);
+
+ if (drive_no > 7) {
+ EC_CONFIG_ERR(sc, "Invalid drive number!\n");
+ return -EINVAL;
+ }
if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) {
EC_CONFIG_ERR(sc, "AL state for IDN config"
@@ -967,6 +973,7 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
}
ec_soe_request_init(req);
+ ec_soe_request_set_drive_no(req, drive_no);
ec_soe_request_set_idn(req, idn);
req->al_state = state;
diff --git a/master/soe_request.c b/master/soe_request.c
index a3a058df7b495a502eddf5f871471ad5672551db..b2fe0716c3b557000bf4fc077ac5ef7b89f093f1 100644
--- a/master/soe_request.c
+++ b/master/soe_request.c
@@ -56,6 +56,8 @@ void ec_soe_request_init(
ec_soe_request_t *req /**< SoE request. */
)
{
+ req->drive_no = 0x00;
+ req->idn = 0x0000;
req->al_state = EC_AL_STATE_INIT;
req->data = NULL;
req->mem_size = 0;
@@ -86,6 +88,7 @@ int ec_soe_request_copy(
const ec_soe_request_t *other /**< Other SoE request to copy from. */
)
{
+ req->drive_no = other->drive_no;
req->idn = other->idn;
req->al_state = other->al_state;
return ec_soe_request_copy_data(req, other->data, other->data_size);
@@ -93,6 +96,18 @@ int ec_soe_request_copy(
/*****************************************************************************/
+/** Set drive number.
+ */
+void ec_soe_request_set_drive_no(
+ ec_soe_request_t *req, /**< SoE request. */
+ uint8_t drive_no /** Drive Number. */
+ )
+{
+ req->drive_no = drive_no;
+}
+
+/*****************************************************************************/
+
/** Set IDN.
*/
void ec_soe_request_set_idn(
diff --git a/master/soe_request.h b/master/soe_request.h
index 40d0fe7fef3bff19f29910448b6244fb07c5e00a..40f61aa677a8489c8a964bcd56d450c55ffaf904 100644
--- a/master/soe_request.h
+++ b/master/soe_request.h
@@ -47,6 +47,7 @@
*/
typedef struct {
struct list_head list; /**< List item. */
+ uint8_t drive_no; /**< Drive number. */
uint16_t idn; /**< Sercos ID-Number. */
ec_al_state_t al_state; /**< AL state (only valid for IDN config). */
uint8_t *data; /**< Pointer to SDO data. */
@@ -66,6 +67,7 @@ void ec_soe_request_init(ec_soe_request_t *);
void ec_soe_request_clear(ec_soe_request_t *);
int ec_soe_request_copy(ec_soe_request_t *, const ec_soe_request_t *);
+void ec_soe_request_set_drive_no(ec_soe_request_t *, uint8_t);
void ec_soe_request_set_idn(ec_soe_request_t *, uint16_t);
int ec_soe_request_alloc(ec_soe_request_t *, size_t);
int ec_soe_request_copy_data(ec_soe_request_t *, const uint8_t *, size_t);