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

Implemented SDO reading via Sysfs.

This commit is contained in:
Florian Pose 2006-10-20 13:05:48 +00:00
parent 2dc4d186c3
commit 93658f687b
7 changed files with 632 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
master/canopen.c
View File

@ -41,6 +41,7 @@
#include <linux/module.h>
#include "canopen.h"
#include "master.h"
/*****************************************************************************/
@ -50,11 +51,16 @@ ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *,
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 kobj_type ktype_ec_sdo = {
static struct attribute *sdo_entry_def_attrs[] = {
&attr_info,
&attr_value,
NULL,
};
@ -101,6 +108,7 @@ int ec_sdo_init(ec_sdo_t *sdo, /**< SDO */
ec_slave_t *slave /**< parent slave */
)
{
sdo->slave = slave;
sdo->index = index;
sdo->object_code = 0x00;
sdo->name = NULL;
@ -185,6 +193,7 @@ int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< SDO entry */
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_info(ec_sdo_entry_t *entry, /**< SDO entry */
/*****************************************************************************/
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 @@ ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */
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 */
)
/**
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 */
)
{
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;
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);
}
#endif
/*****************************************************************************/

19
master/canopen.h
View File

@ -57,6 +57,7 @@ typedef struct
{
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 @@ typedef struct
{
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 @@ ec_sdo_data_t;
/*****************************************************************************/
/**
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 *);

478
master/fsm.c
View File

@ -55,6 +55,7 @@ void ec_fsm_master_configure_slave(ec_fsm_t *);
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_request(ec_fsm_t *);
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 @@ void ec_fsm_master_action_process_states(ec_fsm_t *fsm
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 @@ void ec_fsm_master_action_process_states(ec_fsm_t *fsm
// 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 @@ void ec_fsm_master_sdodict(ec_fsm_t *fsm /**< finite state machine */)
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
*****************************************************************************/
@ -2990,6 +3058,416 @@ void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */)
/*****************************************************************************/
/**
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,

2
master/fsm.h
View File

@ -85,6 +85,8 @@ struct ec_fsm
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 */
};
/*****************************************************************************/

21
master/master.c
View File

@ -130,6 +130,8 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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 @@ void ec_master_reset(ec_master_t *master /**< EtherCAT master */)
ec_master_eoe_stop(master);
ec_master_idle_stop(master);
ec_master_flush_sdo_requests(master);
ec_master_clear_slaves(master);
// empty datagram queue
@ -292,6 +295,24 @@ void ec_master_clear_slaves(ec_master_t *master)
/*****************************************************************************/
/**
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.
*/

5
master/master.h
View File

@ -44,6 +44,7 @@
#include <linux/list.h>
#include <linux/sysfs.h>
#include <linux/timer.h>
#include <linux/wait.h>
#include <asm/atomic.h>
#include <asm/semaphore.h>
@ -137,6 +138,9 @@ struct ec_master
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 @@ int ec_master_measure_bus_time(ec_master_t *);
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 *);
/*****************************************************************************/

1
master/module.c
View File

@ -401,6 +401,7 @@ void ecdev_stop(unsigned int master_index /**< master index */)
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");