diff --git a/TODO b/TODO index 003eb683..5fe2e3e7 100644 --- a/TODO +++ b/TODO @@ -25,7 +25,6 @@ Version 1.4.0: * Adapt remaining examples. * Separate Pdo and Pdo entry classes. * Attach Pdo names. -* Replace ec_sdo_data_t with ec_sdo_request_t. * Make all define underscores the same. Future issues: diff --git a/master/fsm_coe.c b/master/fsm_coe.c index e355172d..34f3a053 100644 --- a/master/fsm_coe.c +++ b/master/fsm_coe.c @@ -183,13 +183,14 @@ void ec_fsm_coe_dictionary(ec_fsm_coe_t *fsm, /**< finite state machine */ Starts to download an Sdo to a slave. */ -void ec_fsm_coe_download(ec_fsm_coe_t *fsm, /**< finite state machine */ - ec_slave_t *slave, /**< EtherCAT slave */ - ec_sdo_data_t *sdodata /**< Sdo data object */ - ) +void ec_fsm_coe_download( + ec_fsm_coe_t *fsm, /**< State machine. */ + ec_slave_t *slave, /**< EtherCAT slave. */ + ec_sdo_request_t *request /**< Sdo request. */ + ) { fsm->slave = slave; - fsm->sdodata = sdodata; + fsm->request = request; fsm->state = ec_fsm_coe_down_start; } @@ -954,12 +955,12 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */) { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; - ec_sdo_data_t *sdodata = fsm->sdodata; + ec_sdo_request_t *request = fsm->request; uint8_t *data; if (fsm->slave->master->debug_level) EC_DBG("Downloading Sdo 0x%04X:%i to slave %i.\n", - sdodata->index, sdodata->subindex, slave->ring_position); + request->index, request->subindex, slave->ring_position); if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); @@ -967,14 +968,14 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */) return; } - if (slave->sii.rx_mailbox_size < 6 + 10 + sdodata->size) { + if (slave->sii.rx_mailbox_size < 6 + 10 + request->data_size) { EC_ERR("Sdo fragmenting not supported yet!\n"); fsm->state = ec_fsm_coe_error; return; } if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, - sdodata->size + 10))) { + request->data_size + 10))) { fsm->state = ec_fsm_coe_error; return; } @@ -982,10 +983,10 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */) EC_WRITE_U16(data, 0x2 << 12); // Sdo request EC_WRITE_U8 (data + 2, (0x1 // size specified | 0x1 << 5)); // Download request - EC_WRITE_U16(data + 3, sdodata->index); - EC_WRITE_U8 (data + 5, sdodata->subindex); - EC_WRITE_U32(data + 6, sdodata->size); - memcpy(data + 10, sdodata->data, sdodata->size); + EC_WRITE_U16(data + 3, request->index); + EC_WRITE_U8 (data + 5, request->subindex); + EC_WRITE_U32(data + 6, request->data_size); + memcpy(data + 10, request->data, request->data_size); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_coe_down_request; @@ -1092,7 +1093,7 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) ec_slave_t *slave = fsm->slave; uint8_t *data, mbox_prot; size_t rec_size; - ec_sdo_data_t *sdodata = fsm->sdodata; + ec_sdo_request_t *request = fsm->request; if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; // FIXME: request again? @@ -1136,7 +1137,7 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request fsm->state = ec_fsm_coe_error; EC_ERR("Sdo download 0x%04X:%X (%i bytes) aborted on slave %i.\n", - sdodata->index, sdodata->subindex, sdodata->size, + request->index, request->subindex, request->data_size, slave->ring_position); if (rec_size < 10) { EC_ERR("Incomplete Abort command:\n"); @@ -1149,11 +1150,11 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response - EC_READ_U16(data + 3) != sdodata->index || // index - EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex + EC_READ_U16(data + 3) != request->index || // index + EC_READ_U8 (data + 5) != request->subindex) { // subindex fsm->state = ec_fsm_coe_error; EC_ERR("Sdo download 0x%04X:%X (%i bytes) failed:\n", - sdodata->index, sdodata->subindex, sdodata->size); + request->index, request->subindex, request->data_size); EC_ERR("Invalid Sdo download response at slave %i!\n", slave->ring_position); ec_print_data(data, rec_size); @@ -1368,12 +1369,6 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) return; } - if (request->data) { - kfree(request->data); - request->data = NULL; - } - request->size = 0; - // normal or expedited? expedited = EC_READ_U8(data + 2) & 0x02; @@ -1414,18 +1409,10 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) return; } - if (!(request->data = (uint8_t *) - kmalloc(complete_size + 1, GFP_ATOMIC))) { - EC_ERR("Failed to allocate %i bytes of Sdo data!\n", - complete_size); + if (ec_sdo_request_copy_data(request, data + 6, complete_size)) { fsm->state = ec_fsm_coe_error; return; } - request->data[complete_size] = 0x00; // just to be sure... - - memcpy(request->data, data + 6, complete_size); - request->size = complete_size; - } else { // normal if (rec_size < 10) { EC_ERR("Received currupted Sdo normal upload" @@ -1458,17 +1445,16 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) return; } - if (!(request->data = (uint8_t *) - kmalloc(complete_size + 1, GFP_ATOMIC))) { - EC_ERR("Failed to allocate %i bytes of Sdo data!\n", - complete_size); + if (ec_sdo_request_alloc(request, complete_size)) { + fsm->state = ec_fsm_coe_error; + return; + } + + if (ec_sdo_request_copy_data(request, data + 10, data_size)) { fsm->state = ec_fsm_coe_error; return; } - request->data[complete_size] = 0x00; // just to be sure... - memcpy(request->data, data + 10, data_size); - request->size = complete_size; fsm->toggle = 0; if (data_size < complete_size) { @@ -1592,6 +1578,7 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) /** CoE state: UP RESPONSE. \todo Timeout behavior + \todo Check for \a data_size exceeding \a complete_size. */ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) @@ -1677,8 +1664,8 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) data_size, seg_size); } - memcpy(request->data + request->size, data + 10, data_size); - request->size += data_size; + memcpy(request->data + request->data_size, data + 10, data_size); + request->data_size += data_size; if (!last_segment) { fsm->toggle = !fsm->toggle; diff --git a/master/fsm_coe.h b/master/fsm_coe.h index ddb593eb..758aa681 100644 --- a/master/fsm_coe.h +++ b/master/fsm_coe.h @@ -60,7 +60,6 @@ struct ec_fsm_coe { unsigned int retries; /**< retries upon datagram timeout */ void (*state)(ec_fsm_coe_t *); /**< CoE state function */ - ec_sdo_data_t *sdodata; /**< input/output: Sdo data object */ cycles_t cycles_start; /**< CoE timestamp */ ec_sdo_t *sdo; /**< current Sdo */ uint8_t subindex; /**< current subindex */ @@ -74,7 +73,7 @@ void ec_fsm_coe_init(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_clear(ec_fsm_coe_t *); void ec_fsm_coe_dictionary(ec_fsm_coe_t *, ec_slave_t *); -void ec_fsm_coe_download(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_data_t *); +void ec_fsm_coe_download(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_request_t *); void ec_fsm_coe_upload(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_request_t *); int ec_fsm_coe_exec(ec_fsm_coe_t *); diff --git a/master/fsm_coe_map.c b/master/fsm_coe_map.c index bc8a3e2b..00a5a117 100644 --- a/master/fsm_coe_map.c +++ b/master/fsm_coe_map.c @@ -181,7 +181,8 @@ void ec_fsm_coe_map_action_next_dir( ec_pdo_mapping_clear_pdos(&fsm->mapping); - ec_sdo_request_read(&fsm->request, fsm->sync_sdo_index, 0); + ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0); + ec_sdo_request_read(&fsm->request); fsm->state = ec_fsm_coe_map_state_pdo_count; ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately @@ -235,8 +236,9 @@ void ec_fsm_coe_map_action_next_pdo( ) { if (fsm->sync_subindex <= fsm->sync_subindices) { - ec_sdo_request_read(&fsm->request, fsm->sync_sdo_index, + ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, fsm->sync_subindex); + ec_sdo_request_read(&fsm->request); fsm->state = ec_fsm_coe_map_state_pdo; ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately @@ -292,7 +294,8 @@ void ec_fsm_coe_map_state_pdo( list_add_tail(&fsm->pdo->list, &fsm->mapping.pdos); - ec_sdo_request_read(&fsm->request, fsm->pdo->index, 0); + ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); + ec_sdo_request_read(&fsm->request); fsm->state = ec_fsm_coe_map_state_pdo_entry_count; ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately @@ -338,7 +341,8 @@ void ec_fsm_coe_map_action_next_pdo_entry( ) { if (fsm->pdo_subindex <= fsm->pdo_subindices) { - ec_sdo_request_read(&fsm->request, fsm->pdo->index, fsm->pdo_subindex); + ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex); + ec_sdo_request_read(&fsm->request); fsm->state = ec_fsm_coe_map_state_pdo_entry; ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately diff --git a/master/fsm_pdo_config.c b/master/fsm_pdo_config.c index 35dfb50a..2bff906e 100644 --- a/master/fsm_pdo_config.c +++ b/master/fsm_pdo_config.c @@ -65,7 +65,7 @@ void ec_fsm_pdo_config_init( ) { fsm->fsm_coe = fsm_coe; - fsm->sdodata.data = (uint8_t *) &fsm->sdo_value; + ec_sdo_request_init(&fsm->request); } /*****************************************************************************/ @@ -76,6 +76,7 @@ void ec_fsm_pdo_config_clear( ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */ ) { + ec_sdo_request_clear(&fsm->request); } /*****************************************************************************/ @@ -204,17 +205,22 @@ void ec_fsm_pdo_config_next_pdo( fsm->pdo->index, fsm->slave->ring_position); } + if (ec_sdo_request_alloc(&fsm->request, 4)) { + fsm->state = ec_fsm_pdo_config_state_error; + return; + } + // set mapped Pdo count to zero - fsm->sdodata.index = fsm->pdo->index; - fsm->sdodata.subindex = 0; // number of configured entries - EC_WRITE_U8(&fsm->sdo_value, 0); - fsm->sdodata.size = 1; + EC_WRITE_U8(&fsm->request.data, 0); + fsm->request.data_size = 1; + ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); + ec_sdo_request_write(&fsm->request); if (fsm->slave->master->debug_level) EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n", fsm->pdo->index); fsm->state = ec_fsm_pdo_config_state_zero_count; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately } @@ -243,18 +249,18 @@ void ec_fsm_pdo_config_add_entry( { uint32_t value; - fsm->sdodata.subindex = fsm->entry_count; value = fsm->entry->index << 16 | fsm->entry->subindex << 8 | fsm->entry->bit_length; - EC_WRITE_U32(&fsm->sdo_value, value); - fsm->sdodata.size = 4; - + EC_WRITE_U32(&fsm->request.data, value); + fsm->request.data_size = 4; + ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count); + ec_sdo_request_write(&fsm->request); if (fsm->slave->master->debug_level) EC_DBG("Configuring Pdo entry %08X at position %u.\n", value, fsm->entry_count); fsm->state = ec_fsm_pdo_config_state_add_entry; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately } @@ -311,16 +317,16 @@ void ec_fsm_pdo_config_state_add_entry( // find next entry if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) { // No more entries to add. Write entry count. - fsm->sdodata.subindex = 0; - EC_WRITE_U8(&fsm->sdo_value, fsm->entry_count); - fsm->sdodata.size = 1; - + EC_WRITE_U8(&fsm->request.data, fsm->entry_count); + fsm->request.data_size = 1; + ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); + ec_sdo_request_write(&fsm->request); if (fsm->slave->master->debug_level) EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_count); fsm->state = ec_fsm_pdo_config_state_entry_count; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately return; } diff --git a/master/fsm_pdo_config.h b/master/fsm_pdo_config.h index 716a0533..a3f1b898 100644 --- a/master/fsm_pdo_config.h +++ b/master/fsm_pdo_config.h @@ -63,8 +63,7 @@ struct ec_fsm_pdo_config const ec_pdo_t *pdo; /**< Current Pdo to configure. */ const ec_pdo_entry_t *entry; /**< Current entry. */ - ec_sdo_data_t sdodata; /**< Sdo configuration data. */ - uint16_t sdo_value; /**< Sdo value. */ + ec_sdo_request_t request; /**< Sdo request. */ unsigned int entry_count; /**< Number of configured entries. */ }; diff --git a/master/fsm_pdo_mapping.c b/master/fsm_pdo_mapping.c index cf60ff04..87de629a 100644 --- a/master/fsm_pdo_mapping.c +++ b/master/fsm_pdo_mapping.c @@ -65,7 +65,7 @@ void ec_fsm_pdo_mapping_init( ) { fsm->fsm_coe = fsm_coe; - fsm->sdodata.data = (uint8_t *) &fsm->sdo_value; + ec_sdo_request_init(&fsm->request); } /*****************************************************************************/ @@ -76,6 +76,7 @@ void ec_fsm_pdo_mapping_clear( ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ ) { + ec_sdo_request_clear(&fsm->request); } /*****************************************************************************/ @@ -195,16 +196,21 @@ void ec_fsm_pdo_mapping_next_dir( fsm->sync->index, fsm->slave->ring_position); } + if (ec_sdo_request_alloc(&fsm->request, 2)) { + fsm->state = ec_fsm_pdo_mapping_state_error; + return; + } + // set mapped Pdo count to zero - fsm->sdodata.index = 0x1C10 + fsm->sync->index; - fsm->sdodata.subindex = 0; // mapped Pdo count - EC_WRITE_U8(&fsm->sdo_value, 0); // zero Pdos mapped - fsm->sdodata.size = 1; + EC_WRITE_U8(&fsm->request.data, 0); // zero Pdos mapped + fsm->request.data_size = 1; + ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0); + ec_sdo_request_write(&fsm->request); if (fsm->slave->master->debug_level) EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync->index); fsm->state = ec_fsm_pdo_mapping_state_zero_count; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately return; } @@ -238,16 +244,17 @@ void ec_fsm_pdo_mapping_add_pdo( ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ ) { - fsm->sdodata.subindex = fsm->pdo_count; - EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); - fsm->sdodata.size = 2; - + EC_WRITE_U16(&fsm->request.data, fsm->pdo->index); + fsm->request.data_size = 2; + ec_sdo_request_address(&fsm->request, + 0x1C10 + fsm->sync->index, fsm->pdo_count); + ec_sdo_request_write(&fsm->request); if (fsm->slave->master->debug_level) EC_DBG("Mapping Pdo 0x%04X at position %u.\n", - fsm->pdo->index, fsm->sdodata.subindex); + fsm->pdo->index, fsm->pdo_count); fsm->state = ec_fsm_pdo_mapping_state_add_pdo; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately } @@ -304,16 +311,16 @@ void ec_fsm_pdo_mapping_state_add_pdo( // find next Pdo if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) { // no more Pdos to map. write Pdo count - fsm->sdodata.subindex = 0; - EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count); - fsm->sdodata.size = 1; - + EC_WRITE_U8(&fsm->request.data, fsm->pdo_count); + fsm->request.data_size = 1; + ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0); + ec_sdo_request_write(&fsm->request); if (fsm->slave->master->debug_level) EC_DBG("Setting number of mapped Pdos to %u.\n", fsm->pdo_count); fsm->state = ec_fsm_pdo_mapping_state_pdo_count; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately return; } diff --git a/master/fsm_pdo_mapping.h b/master/fsm_pdo_mapping.h index 8075d9ea..33eb02e7 100644 --- a/master/fsm_pdo_mapping.h +++ b/master/fsm_pdo_mapping.h @@ -67,8 +67,7 @@ struct ec_fsm_pdo_mapping const ec_pdo_mapping_t *mapping; /**< Target Pdo mapping. */ const ec_pdo_t *pdo; /**< Current Pdo. */ - ec_sdo_data_t sdodata; /**< Sdo configuration data. */ - uint16_t sdo_value; /**< Sdo value. */ + ec_sdo_request_t request; /**< Sdo request. */ unsigned int pdo_count; /**< Number of mapped Pdos. */ }; diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index 33bbd446..dbec63ab 100644 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -448,9 +448,10 @@ void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *fsm /**< slave st // start Sdo configuration fsm->state = ec_fsm_slave_config_state_sdo_conf; - fsm->sdodata = - list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list); - ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); + fsm->request = list_entry(fsm->slave->config->sdo_configs.next, + ec_sdo_request_t, list); + ec_sdo_request_write(fsm->request); + ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->request); ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately } @@ -475,10 +476,11 @@ void ec_fsm_slave_config_state_sdo_conf( } // Another Sdo to configure? - if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) { - fsm->sdodata = - list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list); - ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); + if (fsm->request->list.next != &fsm->slave->config->sdo_configs) { + fsm->request = list_entry(fsm->request->list.next, ec_sdo_request_t, + list); + ec_sdo_request_write(fsm->request); + ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->request); ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately return; } diff --git a/master/fsm_slave_config.h b/master/fsm_slave_config.h index 6f4ed824..cbcca367 100644 --- a/master/fsm_slave_config.h +++ b/master/fsm_slave_config.h @@ -65,7 +65,7 @@ struct ec_fsm_slave_config unsigned int retries; /**< Retries on datagram timeout. */ void (*state)(ec_fsm_slave_config_t *); /**< State function. */ - ec_sdo_data_t *sdodata; /**< Sdo configuration data. */ + ec_sdo_request_t *request; /**< Sdo request for Sdo configuration. */ ec_fsm_change_t fsm_change; /**< State change state machine. */ ec_fsm_coe_t fsm_coe; /**< CoE state machine. */ ec_fsm_pdo_mapping_t fsm_pdo_map; /**< Pdo mapping state machine. */ diff --git a/master/sdo.h b/master/sdo.h index 88385c19..b4c39765 100644 --- a/master/sdo.h +++ b/master/sdo.h @@ -64,20 +64,6 @@ struct ec_sdo { /*****************************************************************************/ -/** CANopen Sdo configuration data. - * \todo remove - */ -typedef struct { - struct list_head list; /**< list item */ - uint16_t index; /**< Sdo index */ - uint8_t subindex; /**< Sdo subindex */ - uint8_t *data; /**< pointer to Sdo data */ - size_t size; /**< size of Sdo data */ -} -ec_sdo_data_t; - -/*****************************************************************************/ - int ec_sdo_init(ec_sdo_t *, uint16_t, ec_slave_t *); void ec_sdo_destroy(ec_sdo_t *); diff --git a/master/sdo_entry.c b/master/sdo_entry.c index 6fffa540..ea198090 100644 --- a/master/sdo_entry.c +++ b/master/sdo_entry.c @@ -242,7 +242,7 @@ not_fit: "Invalid bit length %u for data type 0x%04X. Data:\n", entry->bit_length, entry->data_type); raw_data: - for (i = 0; i < request->size; i++) + for (i = 0; i < request->data_size; i++) off += sprintf(buffer + off, "%02X (%c)\n", request->data[i], request->data[i]); return off; @@ -268,7 +268,8 @@ ssize_t ec_sdo_entry_read_value( request.slave = entry->sdo->slave; ec_sdo_request_init(&request.req); - ec_sdo_request_read(&request.req, entry->sdo->index, entry->subindex); + ec_sdo_request_address(&request.req, entry->sdo->index, entry->subindex); + ec_sdo_request_read(&request.req); // schedule request. down(&master->sdo_sem); diff --git a/master/sdo_request.c b/master/sdo_request.c index 7ea4100e..69761b2a 100644 --- a/master/sdo_request.c +++ b/master/sdo_request.c @@ -55,7 +55,8 @@ void ec_sdo_request_init( ) { req->data = NULL; - req->size = 0; + req->mem_size = 0; + req->data_size = 0; req->state = EC_REQUEST_COMPLETE; } @@ -83,14 +84,15 @@ void ec_sdo_request_clear_data( req->data = NULL; } - req->size = 0; + req->mem_size = 0; + req->data_size = 0; } /*****************************************************************************/ -/** Start an Sdo read operation (download). +/** Set the Sdo address. */ -void ec_sdo_request_read( +void ec_sdo_request_address( ec_sdo_request_t *req, /**< Sdo request. */ uint16_t index, /**< Sdo index. */ uint8_t subindex /**< Sdo subindex. */ @@ -98,9 +100,74 @@ void ec_sdo_request_read( { req->index = index; req->subindex = subindex; - req->state = EC_REQUEST_QUEUED; - - ec_sdo_request_clear_data(req); +} + +/*****************************************************************************/ + +/** Pre-allocates the data memory. + * + * If the \a mem_size is already bigger than \a size, nothing is done. + */ +int ec_sdo_request_alloc( + ec_sdo_request_t *req, /**< Sdo request. */ + size_t size /**< Data size to allocate. */ + ) +{ + if (size <= req->mem_size) + return 0; + + ec_sdo_request_clear_data(req); + + if (!(req->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) { + EC_ERR("Failed to allocate %u bytes of Sdo memory.\n", size); + return -1; + } + + req->mem_size = size; + req->data_size = 0; + return 0; +} + +/*****************************************************************************/ + +/** Copies Sdo data from an external source. + * + * If the \a mem_size is to small, new memory is allocated. + */ +int ec_sdo_request_copy_data( + ec_sdo_request_t *req, /**< Sdo request. */ + const uint8_t *source, /**< Source data. */ + size_t size /**< Number of bytes in \a source. */ + ) +{ + if (ec_sdo_request_alloc(req, size)) + return -1; + + memcpy(req->data, source, size); + req->data_size = size; + return 0; +} + +/*****************************************************************************/ + +/** Start an Sdo read operation (Sdo upload). + */ +void ec_sdo_request_read( + ec_sdo_request_t *req /**< Sdo request. */ + ) +{ + req->state = EC_REQUEST_QUEUED; +} + +/*****************************************************************************/ + +/** Start an Sdo write operation (Sdo download). + */ +void ec_sdo_request_write( + ec_sdo_request_t *req /**< Sdo request. */ + ) +{ + req->state = EC_REQUEST_QUEUED; } /*****************************************************************************/ diff --git a/master/sdo_request.h b/master/sdo_request.h index 59654355..dd71d1ce 100644 --- a/master/sdo_request.h +++ b/master/sdo_request.h @@ -54,7 +54,8 @@ typedef struct { uint16_t index; /**< Sdo index. */ uint8_t subindex; /**< Sdo subindex. */ uint8_t *data; /**< Pointer to Sdo data. */ - size_t size; /**< Size of Sdo data. */ + size_t mem_size; /**< Size of Sdo data memory. */ + size_t data_size; /**< Size of Sdo data. */ ec_request_state_t state; /**< Sdo request state. */ } ec_sdo_request_t; @@ -63,7 +64,12 @@ typedef struct { void ec_sdo_request_init(ec_sdo_request_t *); void ec_sdo_request_clear(ec_sdo_request_t *); -void ec_sdo_request_read(ec_sdo_request_t *, uint16_t, uint8_t); +void ec_sdo_request_address(ec_sdo_request_t *, uint16_t, uint8_t); +int ec_sdo_request_alloc(ec_sdo_request_t *, size_t); +int ec_sdo_request_copy_data(ec_sdo_request_t *, const uint8_t *, size_t); + +void ec_sdo_request_read(ec_sdo_request_t *); +void ec_sdo_request_write(ec_sdo_request_t *); /*****************************************************************************/ diff --git a/master/slave_config.c b/master/slave_config.c index 6593d0a8..e24355c6 100644 --- a/master/slave_config.c +++ b/master/slave_config.c @@ -159,7 +159,7 @@ void ec_slave_config_clear(struct kobject *kobj /**< kobject of the config. */) { ec_slave_config_t *sc; ec_direction_t dir; - ec_sdo_data_t *sdodata, *next_sdodata; + ec_sdo_request_t *req, *next_req; sc = container_of(kobj, ec_slave_config_t, kobj); @@ -168,14 +168,12 @@ void ec_slave_config_clear(struct kobject *kobj /**< kobject of the config. */) ec_pdo_mapping_clear(&sc->mapping[dir]); // free all Sdo configurations - list_for_each_entry_safe(sdodata, next_sdodata, &sc->sdo_configs, list) { - list_del(&sdodata->list); - kfree(sdodata->data); - kfree(sdodata); + list_for_each_entry_safe(req, next_req, &sc->sdo_configs, list) { + list_del(&req->list); + ec_sdo_request_clear(req); + kfree(req); } - /** \todo */ - kfree(sc); } @@ -234,7 +232,7 @@ ssize_t ec_slave_config_info( const ec_pdo_t *pdo; const ec_pdo_entry_t *entry; char str[20]; - ec_sdo_data_t *sdodata; + const ec_sdo_request_t *req; buf += sprintf(buf, "Alias: 0x%04X (%u)\n", sc->alias, sc->alias); buf += sprintf(buf, "Position: %u\n", sc->position); @@ -265,15 +263,15 @@ ssize_t ec_slave_config_info( if (!list_empty((struct list_head *) &sc->sdo_configs)) { buf += sprintf(buf, "\nSdo configurations:\n"); - list_for_each_entry(sdodata, &sc->sdo_configs, list) { - switch (sdodata->size) { - case 1: sprintf(str, "%u", EC_READ_U8(sdodata->data)); break; - case 2: sprintf(str, "%u", EC_READ_U16(sdodata->data)); break; - case 4: sprintf(str, "%u", EC_READ_U32(sdodata->data)); break; + list_for_each_entry(req, &sc->sdo_configs, list) { + switch (req->data_size) { + case 1: sprintf(str, "%u", EC_READ_U8(req->data)); break; + case 2: sprintf(str, "%u", EC_READ_U16(req->data)); break; + case 4: sprintf(str, "%u", EC_READ_U32(req->data)); break; default: sprintf(str, "(invalid size)"); break; } buf += sprintf(buf, " 0x%04X:%-3i -> %s\n", - sdodata->index, sdodata->subindex, str); + req->index, req->subindex, str); } buf += sprintf(buf, "\n"); } @@ -310,31 +308,29 @@ int ec_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, uint8_t subindex, const uint8_t *data, size_t size) { ec_slave_t *slave = sc->slave; - ec_sdo_data_t *sdodata; + ec_sdo_request_t *req; if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); return -1; } - if (!(sdodata = (ec_sdo_data_t *) - kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for Sdo configuration object!\n"); + if (!(req = (ec_sdo_request_t *) + kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate memory for Sdo configuration!\n"); return -1; } - if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) { - EC_ERR("Failed to allocate memory for Sdo configuration data!\n"); - kfree(sdodata); + ec_sdo_request_init(req); + ec_sdo_request_address(req, index, subindex); + + if (ec_sdo_request_copy_data(req, data, size)) { + ec_sdo_request_clear(req); + kfree(req); return -1; } - - sdodata->index = index; - sdodata->subindex = subindex; - memcpy(sdodata->data, data, size); - sdodata->size = size; - - list_add_tail(&sdodata->list, &sc->sdo_configs); + + list_add_tail(&req->list, &sc->sdo_configs); return 0; }