# HG changeset patch # User Florian Pose # Date 1279615951 -7200 # Node ID 7d9fb723fc4ba1250fa4dd20a8da34f42c3c66d7 # Parent a1a6d7719f97e9d88f7a421321852f8df0bde897 Added drive_no parameter to SoE layer. diff -r a1a6d7719f97 -r 7d9fb723fc4b include/ecrt.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). */ diff -r a1a6d7719f97 -r 7d9fb723fc4b lib/master.c --- 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; diff -r a1a6d7719f97 -r 7d9fb723fc4b lib/slave_config.c --- 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; diff -r a1a6d7719f97 -r 7d9fb723fc4b master/cdev.c --- 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; diff -r a1a6d7719f97 -r 7d9fb723fc4b master/fsm_soe.c --- 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"); diff -r a1a6d7719f97 -r 7d9fb723fc4b master/ioctl.h --- 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; diff -r a1a6d7719f97 -r 7d9fb723fc4b master/master.c --- 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); diff -r a1a6d7719f97 -r 7d9fb723fc4b master/slave_config.c --- 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; diff -r a1a6d7719f97 -r 7d9fb723fc4b master/soe_request.c --- 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( diff -r a1a6d7719f97 -r 7d9fb723fc4b master/soe_request.h --- 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);