# HG changeset patch # User Florian Pose # Date 1161349548 0 # Node ID 74754f45d5fa98ce90ee622f9f7462ef05e1392e # Parent b21b000e88e3f50e4d095f958903e7ce337c8318 Implemented SDO reading via Sysfs. diff -r b21b000e88e3 -r 74754f45d5fa master/canopen.c --- a/master/canopen.c Fri Oct 20 13:03:01 2006 +0000 +++ b/master/canopen.c Fri Oct 20 13:05:48 2006 +0000 @@ -41,6 +41,7 @@ #include #include "canopen.h" +#include "master.h" /*****************************************************************************/ @@ -50,11 +51,16 @@ void ec_sdo_clear(struct kobject *); void ec_sdo_entry_clear(struct kobject *); +void ec_sdo_request_init_read(ec_sdo_request_t *, ec_sdo_t *, + ec_sdo_entry_t *); +void ec_sdo_request_clear(ec_sdo_request_t *); + /*****************************************************************************/ /** \cond */ EC_SYSFS_READ_ATTR(info); +EC_SYSFS_READ_ATTR(value); static struct attribute *sdo_def_attrs[] = { &attr_info, @@ -74,6 +80,7 @@ static struct attribute *sdo_entry_def_attrs[] = { &attr_info, + &attr_value, NULL, }; @@ -101,6 +108,7 @@ ec_slave_t *slave /**< parent slave */ ) { + sdo->slave = slave; sdo->index = index; sdo->object_code = 0x00; sdo->name = NULL; @@ -185,6 +193,7 @@ ec_sdo_t *sdo /**< parent SDO */ ) { + entry->sdo = sdo; entry->subindex = subindex; entry->data_type = 0x0000; entry->bit_length = 0; @@ -237,6 +246,76 @@ /*****************************************************************************/ +ssize_t ec_sdo_entry_format_data(ec_sdo_entry_t *entry, /**< SDO entry */ + ec_sdo_request_t *request, /**< SDO request */ + char *buffer /**< target buffer */ + ) +{ + off_t off = 0; + unsigned int i; + + if (entry->data_type == 0x0002 && entry->bit_length == 8) { // int8 + off += sprintf(buffer + off, "%i\n", *((int8_t *) request->data)); + } + else if (entry->data_type == 0x0003 && entry->bit_length == 16) { // int16 + off += sprintf(buffer + off, "%i\n", *((int16_t *) request->data)); + } + else if (entry->data_type == 0x0004 && entry->bit_length == 32) { // int32 + off += sprintf(buffer + off, "%i\n", *((int32_t *) request->data)); + } + else if (entry->data_type == 0x0005 && entry->bit_length == 8) { // uint8 + off += sprintf(buffer + off, "%i\n", *((uint8_t *) request->data)); + } + else if (entry->data_type == 0x0006 && entry->bit_length == 16) { // uint16 + off += sprintf(buffer + off, "%i\n", *((uint16_t *) request->data)); + } + else if (entry->data_type == 0x0007 && entry->bit_length == 32) { // uint32 + off += sprintf(buffer + off, "%i\n", *((uint32_t *) request->data)); + } + else if (entry->data_type == 0x0009) { // string + off += sprintf(buffer + off, "%s\n", request->data); + } + else { + for (i = 0; i < request->size; i++) + off += sprintf(buffer + off, "%02X (%c)\n", + request->data[i], request->data[i]); + } + + return off; +} + +/*****************************************************************************/ + +ssize_t ec_sdo_entry_read_value(ec_sdo_entry_t *entry, /**< SDO entry */ + char *buffer /**< target buffer */ + ) +{ + ec_sdo_t *sdo = entry->sdo; + ec_master_t *master = sdo->slave->master; + off_t off = 0; + ec_sdo_request_t request; + + ec_sdo_request_init_read(&request, sdo, entry); + list_add_tail(&request.queue, &master->sdo_requests); + if (wait_event_interruptible(master->sdo_wait_queue, + request.return_code)) { + // interrupted by signal + list_del_init(&request.queue); + } + + if (request.return_code == 1 && request.data) { + off += ec_sdo_entry_format_data(entry, &request, buffer); + } + else if (request.return_code < 0) { + off = -EINVAL; + } + + ec_sdo_request_clear(&request); + return off; +} + +/*****************************************************************************/ + ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */ struct attribute *attr, char *buffer @@ -247,84 +326,40 @@ if (attr == &attr_info) { return ec_sdo_entry_info(entry, buffer); } + else if (attr == &attr_value) { + return ec_sdo_entry_read_value(entry, buffer); + } return 0; } /*****************************************************************************/ -#if 0 -int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */ - uint16_t sdo_index, /**< SDO index */ - uint8_t sdo_subindex, /**< SDO subindex */ - uint8_t *target, /**< memory for value */ - size_t *size /**< target memory size */ - ) -{ - uint8_t *data; - size_t rec_size, data_size; - uint32_t complete_size; - ec_datagram_t datagram; - - ec_datagram_init(&datagram); - - if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6))) - goto err; - - EC_WRITE_U16(data, 0x2 << 12); // SDO request - EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request - EC_WRITE_U16(data + 3, sdo_index); - EC_WRITE_U8 (data + 5, sdo_subindex); - - if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size))) - goto err; - - if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request - EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request - EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", - sdo_index, sdo_subindex, slave->ring_position); - ec_canopen_abort_msg(EC_READ_U32(data + 6)); - goto err; - } - - if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response - EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response - EC_READ_U16(data + 3) != sdo_index || // index - EC_READ_U8 (data + 5) != sdo_subindex) { // subindex - EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex); - EC_ERR("Invalid SDO upload response at slave %i!\n", - slave->ring_position); - ec_print_data(data, rec_size); - goto err; - } - - if (rec_size < 10) { - EC_ERR("Received currupted SDO upload response!\n"); - ec_print_data(data, rec_size); - goto err; - } - - if ((complete_size = EC_READ_U32(data + 6)) > *size) { - EC_ERR("SDO data does not fit into buffer (%i / %i)!\n", - complete_size, *size); - goto err; - } - - data_size = rec_size - 10; - - if (data_size != complete_size) { - EC_ERR("SDO data incomplete - Fragmenting not implemented.\n"); - goto err; - } - - memcpy(target, data + 10, data_size); - - ec_datagram_clear(&datagram); - return 0; - err: - ec_datagram_clear(&datagram); - return -1; -} -#endif - -/*****************************************************************************/ +/** + SDO request constructor. +*/ + +void ec_sdo_request_init_read(ec_sdo_request_t *req, /**< SDO request */ + ec_sdo_t *sdo, /**< SDO */ + ec_sdo_entry_t *entry /**< SDO entry */ + ) +{ + req->sdo = sdo; + req->entry = entry; + req->data = NULL; + req->size = 0; + req->return_code = 0; +} + +/*****************************************************************************/ + +/** + SDO request destructor. +*/ + +void ec_sdo_request_clear(ec_sdo_request_t *req /**< SDO request */) +{ + if (req->data) kfree(req->data); +} + +/*****************************************************************************/ diff -r b21b000e88e3 -r 74754f45d5fa master/canopen.h --- a/master/canopen.h Fri Oct 20 13:03:01 2006 +0000 +++ b/master/canopen.h Fri Oct 20 13:05:48 2006 +0000 @@ -57,6 +57,7 @@ { struct kobject kobj; /**< kobject */ struct list_head list; /**< list item */ + ec_slave_t *slave; /**< parent slave */ uint16_t index; /**< SDO index */ uint8_t object_code; /**< object code */ char *name; /**< SDO name */ @@ -75,6 +76,7 @@ { struct kobject kobj; /**< kobject */ struct list_head list; /**< list item */ + ec_sdo_t *sdo; /**< parent SDO */ uint8_t subindex; /**< entry subindex */ uint16_t data_type; /**< entry data type */ uint16_t bit_length; /**< entry length in bit */ @@ -100,6 +102,23 @@ /*****************************************************************************/ +/** + CANopen SDO request. +*/ + +typedef struct +{ + struct list_head queue; /**< list item */ + ec_sdo_t *sdo; + ec_sdo_entry_t *entry; + uint8_t *data; /**< pointer to SDO data */ + size_t size; /**< size of SDO data */ + int return_code; +} +ec_sdo_request_t; + +/*****************************************************************************/ + int ec_sdo_init(ec_sdo_t *, uint16_t, ec_slave_t *); int ec_sdo_entry_init(ec_sdo_entry_t *, uint8_t, ec_sdo_t *); diff -r b21b000e88e3 -r 74754f45d5fa master/fsm.c --- a/master/fsm.c Fri Oct 20 13:03:01 2006 +0000 +++ b/master/fsm.c Fri Oct 20 13:05:48 2006 +0000 @@ -55,6 +55,7 @@ void ec_fsm_master_scan_slaves(ec_fsm_t *); void ec_fsm_master_write_eeprom(ec_fsm_t *); void ec_fsm_master_sdodict(ec_fsm_t *); +void ec_fsm_master_sdo_request(ec_fsm_t *); void ec_fsm_startup_start(ec_fsm_t *); void ec_fsm_startup_broadcast(ec_fsm_t *); @@ -109,6 +110,14 @@ void ec_fsm_coe_down_check(ec_fsm_t *); void ec_fsm_coe_down_response(ec_fsm_t *); +void ec_fsm_coe_up_start(ec_fsm_t *); +void ec_fsm_coe_up_request(ec_fsm_t *); +void ec_fsm_coe_up_check(ec_fsm_t *); +void ec_fsm_coe_up_response(ec_fsm_t *); +void ec_fsm_coe_up_seg_request(ec_fsm_t *); +void ec_fsm_coe_up_seg_check(ec_fsm_t *); +void ec_fsm_coe_up_seg_response(ec_fsm_t *); + void ec_fsm_end(ec_fsm_t *); void ec_fsm_error(ec_fsm_t *); @@ -568,6 +577,7 @@ ec_master_t *master = fsm->master; ec_slave_t *slave; char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE]; + ec_sdo_request_t *request, *next_request; // check if any slaves are not in the state, they're supposed to be list_for_each_entry(slave, &master->slaves, list) { @@ -604,6 +614,31 @@ // Check, if EoE processing has to be started ec_master_eoe_start(master); + // check, if there are pending SDO requests + list_for_each_entry_safe(request, next_request, + &master->sdo_requests, queue) { + // TODO: critical section! + list_del_init(&request->queue); + + slave = request->sdo->slave; + + if (slave->current_state == EC_SLAVE_STATE_INIT + || !slave->online + || slave->error_flag) { + request->return_code = -1; + wake_up_interruptible(&master->sdo_wait_queue); + continue; + } + + // start uploading SDO + fsm->slave = slave; + fsm->master_state = ec_fsm_master_sdo_request; + fsm->coe_request = request; + fsm->coe_state = ec_fsm_coe_up_start; + fsm->coe_state(fsm); // execute immediately + return; + } + // check, if slaves have an SDO dictionary to read out. list_for_each_entry(slave, &master->slaves, list) { if (!(slave->sii_mailbox_protocols & EC_MBOX_COE) @@ -1055,6 +1090,39 @@ fsm->master_state(fsm); // execute immediately } +/*****************************************************************************/ + +/** + Master state: SDO REQUEST. +*/ + +void ec_fsm_master_sdo_request(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_master_t *master = fsm->master; + ec_sdo_request_t *request = fsm->coe_request; + + fsm->coe_state(fsm); // execute CoE state machine + + if (fsm->coe_state == ec_fsm_error) { + request->return_code = -1; + wake_up_interruptible(&master->sdo_wait_queue); + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + if (fsm->coe_state != ec_fsm_end) return; + + // SDO dictionary fetching finished + + request->return_code = 1; + wake_up_interruptible(&master->sdo_wait_queue); + + // restart master state machine. + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately +} + /****************************************************************************** * slave scan state machine *****************************************************************************/ @@ -2991,6 +3059,416 @@ /*****************************************************************************/ /** + CoE state: UP START. +*/ + +void ec_fsm_coe_up_start(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_sdo_request_t *request = fsm->coe_request; + ec_sdo_t *sdo = request->sdo; + ec_sdo_entry_t *entry = request->entry; + uint8_t *data; + + EC_INFO("Uploading SDO 0x%04X:%i from slave %i.\n", + sdo->index, entry->subindex, slave->ring_position); + + if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 6))) { + fsm->coe_state = ec_fsm_error; + return; + } + + EC_WRITE_U16(data, 0x2 << 12); // SDO request + EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request + EC_WRITE_U16(data + 3, sdo->index); + EC_WRITE_U8 (data + 5, entry->subindex); + + if (master->debug_level) { + EC_DBG("Upload request:\n"); + ec_print_data(data, 6); + } + + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_request; +} + +/*****************************************************************************/ + +/** + CoE state: UP REQUEST. +*/ + +void ec_fsm_coe_up_request(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Reception of CoE upload request failed.\n"); + return; + } + + fsm->coe_start = datagram->cycles_sent; + + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_check; +} + +/*****************************************************************************/ + +/** + CoE state: UP CHECK. +*/ + +void ec_fsm_coe_up_check(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Reception of CoE mailbox check datagram failed.\n"); + return; + } + + if (!ec_slave_mbox_check(datagram)) { + if (datagram->cycles_received + - fsm->coe_start >= (cycles_t) 100 * cpu_khz) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Timeout while checking SDO upload on slave %i.\n", + slave->ring_position); + return; + } + + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + ec_master_queue_datagram(fsm->master, datagram); + return; + } + + // Fetch response + ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_response; +} + +/*****************************************************************************/ + +/** + CoE state: UP RESPONSE. +*/ + +void ec_fsm_coe_up_response(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint8_t *data, mbox_prot; + size_t rec_size, data_size; + ec_sdo_request_t *request = fsm->coe_request; + ec_sdo_t *sdo = request->sdo; + ec_sdo_entry_t *entry = request->entry; + uint32_t complete_size; + unsigned int expedited, size_specified; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Reception of CoE upload response failed.\n"); + return; + } + + if (!(data = ec_slave_mbox_fetch(slave, datagram, + &mbox_prot, &rec_size))) { + fsm->coe_state = ec_fsm_error; + return; + } + + if (master->debug_level) { + EC_DBG("Upload response:\n"); + ec_print_data(data, rec_size); + } + + if (mbox_prot != 0x03) { // CoE + EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + fsm->coe_state = ec_fsm_error; + return; + } + + if (rec_size < 10) { + EC_ERR("Received currupted SDO upload response!\n"); + ec_print_data(data, rec_size); + fsm->coe_state = ec_fsm_error; + return; + } + + if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request + EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request + EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", + sdo->index, entry->subindex, slave->ring_position); + ec_canopen_abort_msg(EC_READ_U32(data + 6)); + fsm->coe_state = ec_fsm_error; + return; + } + + if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response + EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response + EC_READ_U16(data + 3) != sdo->index || // index + EC_READ_U8 (data + 5) != entry->subindex) { // subindex + EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex); + EC_ERR("Invalid SDO upload response at slave %i!\n", + slave->ring_position); + ec_print_data(data, rec_size); + fsm->coe_state = ec_fsm_error; + return; + } + + data_size = rec_size - 10; + expedited = EC_READ_U8(data + 2) & 0x02; + + if (expedited) { + EC_WARN("Received expedited response upon normal request!\n"); + + size_specified = EC_READ_U8(data + 2) & 0x01; + if (size_specified) { + complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); + } + else { + complete_size = 4; + } + } + else { + complete_size = EC_READ_U32(data + 6); + } + + if (request->data) { + kfree(request->data); + request->data = NULL; + } + request->size = 0; + + if (complete_size) { + if (!(request->data = (uint8_t *) kmalloc(complete_size + 1, GFP_ATOMIC))) { + EC_ERR("Failed to allocate %i bytes of SDO data!\n", complete_size); + fsm->coe_state = ec_fsm_error; + return; + } + request->data[complete_size] = 0x00; // just to be sure... + } + + if (expedited) { + memcpy(request->data, data + 6, complete_size); + request->size = complete_size; + } + else { + memcpy(request->data, data + 10, data_size); + request->size = data_size; + fsm->coe_toggle = 0; + + if (data_size < complete_size) { + EC_WARN("SDO data incomplete (%i / %i).\n", data_size, complete_size); + + if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) { + fsm->coe_state = ec_fsm_error; + return; + } + + EC_WRITE_U16(data, 0x2 << 12); // SDO request + EC_WRITE_U8 (data + 2, (fsm->coe_toggle << 4 // toggle + | 0x3 << 5)); // upload segment request + + if (master->debug_level) { + EC_DBG("Upload segment request:\n"); + ec_print_data(data, 3); + } + + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_seg_request; + return; + } + } + + fsm->coe_state = ec_fsm_end; // success +} + +/*****************************************************************************/ + +/** + CoE state: UP REQUEST. +*/ + +void ec_fsm_coe_up_seg_request(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Reception of CoE upload segment request failed.\n"); + return; + } + + fsm->coe_start = datagram->cycles_sent; + + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_seg_check; +} + +/*****************************************************************************/ + +/** + CoE state: UP CHECK. +*/ + +void ec_fsm_coe_up_seg_check(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Reception of CoE mailbox check datagram failed.\n"); + return; + } + + if (!ec_slave_mbox_check(datagram)) { + if (datagram->cycles_received + - fsm->coe_start >= (cycles_t) 100 * cpu_khz) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Timeout while checking SDO upload segment on slave %i.\n", + slave->ring_position); + return; + } + + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + ec_master_queue_datagram(fsm->master, datagram); + return; + } + + // Fetch response + ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_seg_response; +} + +/*****************************************************************************/ + +/** + CoE state: UP RESPONSE. +*/ + +void ec_fsm_coe_up_seg_response(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint8_t *data, mbox_prot; + size_t rec_size, data_size; + ec_sdo_request_t *request = fsm->coe_request; + ec_sdo_t *sdo = request->sdo; + ec_sdo_entry_t *entry = request->entry; + uint32_t seg_size; + unsigned int last_segment; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->coe_state = ec_fsm_error; + EC_ERR("Reception of CoE upload segment response failed.\n"); + return; + } + + if (!(data = ec_slave_mbox_fetch(slave, datagram, + &mbox_prot, &rec_size))) { + fsm->coe_state = ec_fsm_error; + return; + } + + if (master->debug_level) { + EC_DBG("Upload segment response:\n"); + ec_print_data(data, rec_size); + } + + if (mbox_prot != 0x03) { // CoE + EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + fsm->coe_state = ec_fsm_error; + return; + } + + if (rec_size < 10) { + EC_ERR("Received currupted SDO upload segment response!\n"); + ec_print_data(data, rec_size); + fsm->coe_state = ec_fsm_error; + return; + } + + if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request + EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request + EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", + sdo->index, entry->subindex, slave->ring_position); + ec_canopen_abort_msg(EC_READ_U32(data + 6)); + fsm->coe_state = ec_fsm_error; + return; + } + + if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response + EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response + EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex); + EC_ERR("Invalid SDO upload segment response at slave %i!\n", + slave->ring_position); + ec_print_data(data, rec_size); + fsm->coe_state = ec_fsm_error; + return; + } + + last_segment = EC_READ_U8(data + 2) & 0x01; + seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; + data_size = rec_size - 10; + + if (data_size != seg_size) { + EC_WARN("SDO segment data invalid (%i / %i)" + " - Fragmenting not implemented.\n", + data_size, seg_size); + } + + memcpy(request->data + request->size, data + 10, data_size); + request->size += data_size; + + if (!last_segment) { + fsm->coe_toggle = !fsm->coe_toggle; + + if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) { + fsm->coe_state = ec_fsm_error; + return; + } + + EC_WRITE_U16(data, 0x2 << 12); // SDO request + EC_WRITE_U8 (data + 2, (fsm->coe_toggle << 4 // toggle + | 0x3 << 5)); // upload segment request + + if (master->debug_level) { + EC_DBG("Upload segment request:\n"); + ec_print_data(data, 3); + } + + ec_master_queue_datagram(fsm->master, datagram); + fsm->coe_state = ec_fsm_coe_up_seg_request; + return; + } + + fsm->coe_state = ec_fsm_end; // success +} + +/*****************************************************************************/ + +/** SDO abort messages. The "abort SDO transfer request" supplies an abort code, which can be translated to clear text. This table does diff -r b21b000e88e3 -r 74754f45d5fa master/fsm.h --- a/master/fsm.h Fri Oct 20 13:03:01 2006 +0000 +++ b/master/fsm.h Fri Oct 20 13:05:48 2006 +0000 @@ -85,6 +85,8 @@ cycles_t coe_start; /**< CoE timestamp */ ec_sdo_t *coe_sdo; /**< current SDO */ uint8_t coe_subindex; /**< current subindex */ + ec_sdo_request_t *coe_request; /**< SDO request */ + uint8_t coe_toggle; /**< toggle bit for segment commands */ }; /*****************************************************************************/ diff -r b21b000e88e3 -r 74754f45d5fa master/master.c --- a/master/master.c Fri Oct 20 13:03:01 2006 +0000 +++ b/master/master.c Fri Oct 20 13:05:48 2006 +0000 @@ -130,6 +130,8 @@ master->idle_cycle_time_pos = 0; master->eoe_cycle_time_pos = 0; master->debug_level = 0; + INIT_LIST_HEAD(&master->sdo_requests); + init_waitqueue_head(&master->sdo_wait_queue); // create workqueue if (!(master->workqueue = create_singlethread_workqueue("EtherCAT"))) { @@ -237,6 +239,7 @@ ec_master_eoe_stop(master); ec_master_idle_stop(master); + ec_master_flush_sdo_requests(master); ec_master_clear_slaves(master); // empty datagram queue @@ -293,6 +296,24 @@ /*****************************************************************************/ /** + Flushes the SDO request queue. +*/ + +void ec_master_flush_sdo_requests(ec_master_t *master) +{ + ec_sdo_request_t *req, *next_req; + + list_for_each_entry_safe(req, next_req, &master->sdo_requests, queue) { + list_del_init(&req->queue); + req->return_code = -1; + } + + wake_up_interruptible(&master->sdo_wait_queue); +} + +/*****************************************************************************/ + +/** Places a datagram in the datagram queue. */ diff -r b21b000e88e3 -r 74754f45d5fa master/master.h --- a/master/master.h Fri Oct 20 13:03:01 2006 +0000 +++ b/master/master.h Fri Oct 20 13:05:48 2006 +0000 @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -137,6 +138,9 @@ void *cb_data; /**< data parameter of locking callbacks */ uint8_t eeprom_write_enable; /**< allow write operations to EEPROMs */ + + struct list_head sdo_requests; /**< list of SDO read/write requests */ + wait_queue_head_t sdo_wait_queue; /**< wait queue for SDO access */ }; /*****************************************************************************/ @@ -169,6 +173,7 @@ void ec_sync_config(const ec_sii_sync_t *, const ec_slave_t *, uint8_t *); void ec_fmmu_config(const ec_fmmu_t *, const ec_slave_t *, uint8_t *); void ec_master_calc_addressing(ec_master_t *); +void ec_master_flush_sdo_requests(ec_master_t *); /*****************************************************************************/ diff -r b21b000e88e3 -r 74754f45d5fa master/module.c --- a/master/module.c Fri Oct 20 13:03:01 2006 +0000 +++ b/master/module.c Fri Oct 20 13:05:48 2006 +0000 @@ -401,6 +401,7 @@ if (!(master = ec_find_master(master_index))) return; ec_master_idle_stop(master); + ec_master_flush_sdo_requests(master); if (ec_device_close(master->device)) EC_WARN("Failed to close device!\n");