Added drive_no parameter to SoE layer.
authorFlorian Pose <fp@igh-essen.com>
Tue, 20 Jul 2010 10:52:31 +0200
changeset 1952 7d9fb723fc4b
parent 1951 a1a6d7719f97
child 1953 968a531e5a89
Added drive_no parameter to SoE layer.
include/ecrt.h
lib/master.c
lib/slave_config.c
master/cdev.c
master/fsm_soe.c
master/ioctl.h
master/master.c
master/slave_config.c
master/soe_request.c
master/soe_request.h
--- a/include/ecrt.h	Tue Jul 13 13:52:59 2010 +0200
+++ b/include/ecrt.h	Tue Jul 20 10:52:31 2010 +0200
@@ -687,6 +687,7 @@
 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_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 @@
  */
 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). */
--- a/lib/master.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/lib/master.c	Tue Jul 20 10:52:31 2010 +0200
@@ -305,11 +305,13 @@
 /*****************************************************************************/
 
 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_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;
--- a/lib/slave_config.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/lib/slave_config.c	Tue Jul 20 10:52:31 2010 +0200
@@ -472,12 +472,13 @@
 
 /*****************************************************************************/
 
-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;
--- a/master/cdev.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/cdev.c	Tue Jul 20 10:52:31 2010 +0200
@@ -2474,7 +2474,7 @@
     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 @@
     }
 
     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 @@
     }
 
     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;
--- a/master/fsm_soe.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/fsm_soe.c	Tue Jul 20 10:52:31 2010 +0200
@@ -201,7 +201,8 @@
     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 @@
         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 @@
         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 @@
     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");
--- a/master/ioctl.h	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/ioctl.h	Tue Jul 20 10:52:31 2010 +0200
@@ -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 {
     // 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 {
     // 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 {
     // inputs
     uint32_t config_index;
+    uint8_t drive_no;
     uint16_t idn;
     ec_al_state_t al_state;
     const uint8_t *data;
--- a/master/master.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/master.c	Tue Jul 20 10:52:31 2010 +0200
@@ -2345,13 +2345,20 @@
 /*****************************************************************************/
 
 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_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);
 
--- a/master/slave_config.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/slave_config.c	Tue Jul 20 10:52:31 2010 +0200
@@ -938,16 +938,22 @@
 
 /*****************************************************************************/
 
-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 @@
     }
 
     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;
 
--- a/master/soe_request.c	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/soe_request.c	Tue Jul 20 10:52:31 2010 +0200
@@ -56,6 +56,8 @@
         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 @@
         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 @@
 
 /*****************************************************************************/
 
+/** 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(
--- a/master/soe_request.h	Tue Jul 13 13:52:59 2010 +0200
+++ b/master/soe_request.h	Tue Jul 20 10:52:31 2010 +0200
@@ -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_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);