bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Removed ec_sdodata_t; CoE state machines work on ec_sdo_request_t.

This commit is contained in:
Florian Pose 2008-03-03 16:22:39 +00:00
parent 17069bcb70
commit 5ea60febf9
15 changed files with 205 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
TODO
View File

@ -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:

71
master/fsm_coe.c
View File

@ -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;

3
master/fsm_coe.h
View File

@ -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 *);

12
master/fsm_coe_map.c
View File

@ -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

38
master/fsm_pdo_config.c
View File

@ -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;
}

3
master/fsm_pdo_config.h
View File

@ -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. */
};

41
master/fsm_pdo_mapping.c
View File

@ -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;
}

3
master/fsm_pdo_mapping.h
View File

@ -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. */
};

16
master/fsm_slave_config.c
View File

@ -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;
}

2
master/fsm_slave_config.h
View File

@ -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. */

14
master/sdo.h
View File

@ -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 *);

5
master/sdo_entry.c
View File

@ -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);

81
master/sdo_request.c
View File

@ -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;
}
/*****************************************************************************/

10
master/sdo_request.h
View File

@ -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 *);
/*****************************************************************************/

52
master/slave_config.c
View File

@ -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;
}