# HG changeset patch # User Florian Pose # Date 1260793550 -3600 # Node ID eb9185dfa8aceda1241f5675a4a0f7274fa1e6ad # Parent e51cf2af3ff9aa483b34d0cd116ddea540d00c68# Parent 1f640e321ee40b12e1b17d8f239e014011f3fea4 Merged changes by Martin Troxler. diff -r e51cf2af3ff9 -r eb9185dfa8ac include/ecrt.h --- a/include/ecrt.h Mon Dec 14 13:11:36 2009 +0100 +++ b/include/ecrt.h Mon Dec 14 13:25:50 2009 +0100 @@ -698,6 +698,15 @@ ec_master_t *master /**< EtherCAT master. */ ); + +/** Set max. number of databytes in a cycle + * + */ +int ecrt_master_set_max_cycle_size( + ec_master_t *master, /**< EtherCAT master. */ + size_t max_cycle_data_size /**< Max. number of databytes in a cycle */ + ); + /** Sends all datagrams in the queue. * * This method takes all datagrams, that have been queued for transmission, diff -r e51cf2af3ff9 -r eb9185dfa8ac lib/master.c --- a/lib/master.c Mon Dec 14 13:11:36 2009 +0100 +++ b/lib/master.c Mon Dec 14 13:25:50 2009 +0100 @@ -344,6 +344,21 @@ } } + +/*****************************************************************************/ + +int ecrt_master_set_max_cycle_size(ec_master_t *master,size_t max_cycle_data_size) +{ + if (ioctl(master->fd, EC_IOCTL_SET_MAX_CYCLE_SIZE, + &max_cycle_data_size) == -1) { + fprintf(stderr, "Failed to activate master: %s\n", + strerror(errno)); + return -1; // FIXME + } + return 0; +} + + /*****************************************************************************/ void ecrt_master_send(ec_master_t *master) diff -r e51cf2af3ff9 -r eb9185dfa8ac master/Kbuild.in --- a/master/Kbuild.in Mon Dec 14 13:11:36 2009 +0100 +++ b/master/Kbuild.in Mon Dec 14 13:25:50 2009 +0100 @@ -44,6 +44,7 @@ fsm_coe.o \ fsm_foe.o \ fsm_master.o \ + fsm_slave.o \ fsm_pdo.o \ fsm_pdo_entry.o \ fsm_sii.o \ diff -r e51cf2af3ff9 -r eb9185dfa8ac master/Makefile.am --- a/master/Makefile.am Mon Dec 14 13:11:36 2009 +0100 +++ b/master/Makefile.am Mon Dec 14 13:25:50 2009 +0100 @@ -43,6 +43,7 @@ fsm_coe.c fsm_coe.h \ fsm_foe.c fsm_foe.h \ fsm_master.c fsm_master.h \ + fsm_slave.c fsm_slave.h \ fsm_pdo.c fsm_pdo.h \ fsm_pdo_entry.c fsm_pdo_entry.h \ fsm_sii.c fsm_sii.h \ diff -r e51cf2af3ff9 -r eb9185dfa8ac master/cdev.c --- a/master/cdev.c Mon Dec 14 13:11:36 2009 +0100 +++ b/master/cdev.c Mon Dec 14 13:25:50 2009 +0100 @@ -805,6 +805,8 @@ return -EINVAL; } + if (master->debug_level) + EC_DBG("Schedule SDO upload request for slave %u\n",request.slave->ring_position); // schedule request. list_add_tail(&request.list, &master->slave_sdo_requests); @@ -816,7 +818,7 @@ // interrupted by signal down(&master->master_sem); if (request.req.state == EC_INT_REQUEST_QUEUED) { - list_del(&request.req.list); + list_del(&request.list); up(&master->master_sem); ec_sdo_request_clear(&request.req); return -EINTR; @@ -828,6 +830,9 @@ // wait until master FSM has finished processing wait_event(master->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY); + if (master->debug_level) + EC_DBG("Scheduled SDO upload request for slave %u done\n",request.slave->ring_position); + data.abort_code = request.req.abort_code; if (request.req.state != EC_INT_REQUEST_SUCCESS) { @@ -906,6 +911,8 @@ return -EINVAL; } + if (master->debug_level) + EC_DBG("Schedule SDO download request for slave %u\n",request.slave->ring_position); // schedule request. list_add_tail(&request.list, &master->slave_sdo_requests); @@ -917,7 +924,7 @@ // interrupted by signal down(&master->master_sem); if (request.req.state == EC_INT_REQUEST_QUEUED) { - list_del(&request.req.list); + list_del(&request.list); up(&master->master_sem); ec_sdo_request_clear(&request.req); return -EINTR; @@ -929,6 +936,9 @@ // wait until master FSM has finished processing wait_event(master->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY); + if (master->debug_level) + EC_DBG("Scheduled SDO download request for slave %u done\n",request.slave->ring_position); + data.abort_code = request.req.abort_code; retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; @@ -1663,6 +1673,32 @@ return 0; } + +/*****************************************************************************/ + +/** Set max. number of databytes in a cycle + */ +int ec_cdev_ioctl_set_max_cycle_size( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg, /**< ioctl() argument. */ + ec_cdev_priv_t *priv /**< Private data structure of file handle. */ + ) +{ + size_t max_cycle_size; + + if (copy_from_user(&max_cycle_size, (void __user *) arg, sizeof(max_cycle_size))) { + return -EFAULT; + } + + if (down_interruptible(&master->master_sem)) + return -EINTR; + master->max_queue_size = max_cycle_size; + up(&master->master_sem); + + return 0; +} + + /*****************************************************************************/ /** Send frames. @@ -3456,6 +3492,10 @@ return ec_cdev_ioctl_voe_exec(master, arg, priv); case EC_IOCTL_VOE_DATA: return ec_cdev_ioctl_voe_data(master, arg, priv); + case EC_IOCTL_SET_MAX_CYCLE_SIZE: + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_set_max_cycle_size(master,arg,priv); default: return -ENOTTY; } diff -r e51cf2af3ff9 -r eb9185dfa8ac master/fsm_master.c --- a/master/fsm_master.c Mon Dec 14 13:11:36 2009 +0100 +++ b/master/fsm_master.c Mon Dec 14 13:25:50 2009 +0100 @@ -60,6 +60,7 @@ void ec_fsm_master_state_reg_request(ec_fsm_master_t *); void ec_fsm_master_state_foe_request(ec_fsm_master_t *); + /*****************************************************************************/ /** Constructor. @@ -115,18 +116,21 @@ * * If the state machine's datagram is not sent or received yet, the execution * of the state machine is delayed to the next cycle. - */ -void ec_fsm_master_exec( + * + * \return true, if the state machine was executed + */ +int ec_fsm_master_exec( ec_fsm_master_t *fsm /**< Master state machine. */ ) { if (fsm->datagram->state == EC_DATAGRAM_SENT || fsm->datagram->state == EC_DATAGRAM_QUEUED) { // datagram was not sent or received yet. - return; + return 0; } fsm->state(fsm); + return 1; } /*****************************************************************************/ @@ -411,7 +415,6 @@ ec_master_t *master = fsm->master; ec_slave_t *slave; ec_sdo_request_t *req; - ec_master_sdo_request_t *request; // search for internal requests to be processed for (slave = master->slaves; @@ -425,7 +428,7 @@ if (ec_sdo_request_timed_out(req)) { req->state = EC_INT_REQUEST_FAILURE; if (master->debug_level) - EC_DBG("SDO request for slave %u timed out...\n", + EC_DBG("Internal SDO request for slave %u timed out...\n", slave->ring_position); continue; } @@ -437,7 +440,7 @@ req->state = EC_INT_REQUEST_BUSY; if (master->debug_level) - EC_DBG("Processing SDO request for slave %u...\n", + EC_DBG("Processing internal SDO request for slave %u...\n", slave->ring_position); fsm->idle = 0; @@ -450,42 +453,6 @@ } } } - - // search the first external request to be processed - while (1) { - if (list_empty(&master->slave_sdo_requests)) - break; - - // get first request - request = list_entry(master->slave_sdo_requests.next, - ec_master_sdo_request_t, list); - list_del_init(&request->list); // dequeue - request->req.state = EC_INT_REQUEST_BUSY; - - slave = request->slave; - if (slave->current_state == EC_SLAVE_STATE_INIT) { - EC_ERR("Discarding SDO request, slave %u is in INIT.\n", - slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&master->sdo_queue); - continue; - } - - // Found pending SDO request. Execute it! - if (master->debug_level) - EC_DBG("Processing SDO request for slave %u...\n", - slave->ring_position); - - // Start uploading SDO - fsm->idle = 0; - fsm->sdo_request = &request->req; - fsm->slave = slave; - fsm->state = ec_fsm_master_state_sdo_request; - ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); - ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately - return 1; - } - return 0; } @@ -1047,7 +1014,7 @@ if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; if (!ec_fsm_coe_success(&fsm->fsm_coe)) { - EC_DBG("Failed to process SDO request for slave %u.\n", + EC_DBG("Failed to process internal SDO request for slave %u.\n", fsm->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->sdo_queue); @@ -1060,7 +1027,7 @@ wake_up(&master->sdo_queue); if (master->debug_level) - EC_DBG("Finished SDO request for slave %u.\n", + EC_DBG("Finished internal SDO request for slave %u.\n", fsm->slave->ring_position); // check for another SDO request diff -r e51cf2af3ff9 -r eb9185dfa8ac master/fsm_master.h --- a/master/fsm_master.h Mon Dec 14 13:11:36 2009 +0100 +++ b/master/fsm_master.h Mon Dec 14 13:25:50 2009 +0100 @@ -133,7 +133,7 @@ void ec_fsm_master_init(ec_fsm_master_t *, ec_master_t *, ec_datagram_t *); void ec_fsm_master_clear(ec_fsm_master_t *); -void ec_fsm_master_exec(ec_fsm_master_t *); +int ec_fsm_master_exec(ec_fsm_master_t *); int ec_fsm_master_idle(const ec_fsm_master_t *); /*****************************************************************************/ diff -r e51cf2af3ff9 -r eb9185dfa8ac master/fsm_sii.c --- a/master/fsm_sii.c Mon Dec 14 13:11:36 2009 +0100 +++ b/master/fsm_sii.c Mon Dec 14 13:25:50 2009 +0100 @@ -248,7 +248,6 @@ SII state: READ FETCH. Fetches the result of an SII-read datagram. */ - void ec_fsm_sii_state_read_fetch( ec_fsm_sii_t *fsm /**< finite state machine */ ) @@ -302,6 +301,7 @@ } // issue check/fetch datagram again + fsm->datagram->state = EC_DATAGRAM_INIT; fsm->retries = EC_FSM_RETRIES; return; } @@ -435,6 +435,7 @@ #endif // issue check datagram again fsm->retries = EC_FSM_RETRIES; + fsm->datagram->state = EC_DATAGRAM_INIT; return; } @@ -453,6 +454,7 @@ // issue check datagram again fsm->retries = EC_FSM_RETRIES; + fsm->datagram->state = EC_DATAGRAM_INIT; return; } diff -r e51cf2af3ff9 -r eb9185dfa8ac master/fsm_slave.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_slave.c Mon Dec 14 13:25:50 2009 +0100 @@ -0,0 +1,185 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** \file + * EtherCAT slave (SDO) state machine. + */ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "mailbox.h" + +#include "fsm_slave.h" + +/*****************************************************************************/ + +void ec_fsm_slave_state_idle(ec_fsm_slave_t *); +void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); + + +/*****************************************************************************/ + +/** Constructor. + */ +void ec_fsm_slave_init( + ec_fsm_slave_t *fsm, /**< Slave state machine. */ + ec_slave_t *slave, /**< EtherCAT slave. */ + ec_datagram_t *datagram /**< Datagram object to use. */ + ) +{ + fsm->slave = slave; + fsm->datagram = datagram; + fsm->datagram->data_size = 0; + fsm->state = ec_fsm_slave_state_idle; + + // init sub-state-machines + ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); +} + +/*****************************************************************************/ + +/** Destructor. + */ +void ec_fsm_slave_clear( + ec_fsm_slave_t *fsm /**< Master state machine. */ + ) +{ + // clear sub-state machines + ec_fsm_coe_clear(&fsm->fsm_coe); +} + +/*****************************************************************************/ + +/** Executes the current state of the state machine. + * + * If the state machine's datagram is not sent or received yet, the execution + * of the state machine is delayed to the next cycle. + */ +void ec_fsm_slave_exec( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + if (fsm->datagram->state == EC_DATAGRAM_SENT + || fsm->datagram->state == EC_DATAGRAM_QUEUED) { + // datagram was not sent or received yet. + return; + } + + fsm->state(fsm); + return; +} + +/****************************************************************************** + * Slave state machine + *****************************************************************************/ + +/** Slave state: IDLE. + * + * Check for pending SDO requests and process one. + * + */ +void ec_fsm_slave_state_idle( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_master_sdo_request_t *request, *next; + + // search the first matching external request to be processed + list_for_each_entry_safe(request, next, &master->slave_sdo_requests, list) { + if (request->slave != slave) + continue; + list_del_init(&request->list); // dequeue + request->req.state = EC_INT_REQUEST_BUSY; + + if (slave->current_state == EC_SLAVE_STATE_INIT) { + EC_ERR("Discarding SDO request, slave %u is in INIT.\n", + slave->ring_position); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&master->sdo_queue); + continue; + } + + // Found pending SDO request. Execute it! + if (master->debug_level) + EC_DBG("Processing SDO request for slave %u...\n", + slave->ring_position); + + // Start SDO transfer + fsm->sdo_request = &request->req; + fsm->state = ec_fsm_slave_state_sdo_request; + ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); + ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately + ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram); + return; + } +} + +/*****************************************************************************/ + +/** Slave state: SDO_REQUEST. + */ +void ec_fsm_slave_state_sdo_request( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_sdo_request_t *request = fsm->sdo_request; + + if (ec_fsm_coe_exec(&fsm->fsm_coe)) + { + ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram); + return; + } + if (!ec_fsm_coe_success(&fsm->fsm_coe)) { + EC_DBG("Failed to process SDO request for slave %u.\n", + fsm->slave->ring_position); + request->state = EC_INT_REQUEST_FAILURE; + wake_up(&master->sdo_queue); + fsm->sdo_request = NULL; + fsm->state = ec_fsm_slave_state_idle; + return; + } + + // SDO request finished + request->state = EC_INT_REQUEST_SUCCESS; + wake_up(&master->sdo_queue); + + if (master->debug_level) + EC_DBG("Finished SDO request for slave %u.\n", + fsm->slave->ring_position); + + fsm->sdo_request = NULL; + fsm->datagram->data_size = 0; + fsm->state = ec_fsm_slave_state_idle; +} diff -r e51cf2af3ff9 -r eb9185dfa8ac master/fsm_slave.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_slave.h Mon Dec 14 13:25:50 2009 +0100 @@ -0,0 +1,70 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** + \file + EtherCAT slave request (SDO) state machine. +*/ + +/*****************************************************************************/ +#ifndef __EC_FSM_SLAVE_H__ +#define __EC_FSM_SLAVE_H__ + +#include "globals.h" +#include "datagram.h" +#include "sdo_request.h" +#include "fsm_coe.h" + + +typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */ + +/** Finite state machine of an EtherCAT slave. + */ +struct ec_fsm_slave { + ec_slave_t *slave; /**< slave the FSM runs on */ + ec_datagram_t *datagram; /**< datagram used in the state machine */ + + void (*state)(ec_fsm_slave_t *); /**< master state function */ + ec_sdo_request_t *sdo_request; /**< SDO request to process. */ + + ec_fsm_coe_t fsm_coe; /**< CoE state machine */ +}; + +/*****************************************************************************/ + +void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *, ec_datagram_t *); +void ec_fsm_slave_clear(ec_fsm_slave_t *); + +void ec_fsm_slave_exec(ec_fsm_slave_t *); +int ec_fsm_slave_idle(const ec_fsm_slave_t *); + +/*****************************************************************************/ + + +#endif // __EC_FSM_SLAVE_H__ diff -r e51cf2af3ff9 -r eb9185dfa8ac master/globals.h --- a/master/globals.h Mon Dec 14 13:11:36 2009 +0100 +++ b/master/globals.h Mon Dec 14 13:25:50 2009 +0100 @@ -48,6 +48,12 @@ /** Datagram timeout in microseconds. */ #define EC_IO_TIMEOUT 500 +/** SDO injection timeout in microseconds. */ +#define EC_SDO_INJECTION_TIMEOUT 10000 + +/** time to send a byte in nanoseconds. */ +#define EC_BYTE_TRANSMITION_TIME 80LL + /** Number of state machine retries on datagram timeout. */ #define EC_FSM_RETRIES 3 diff -r e51cf2af3ff9 -r eb9185dfa8ac master/ioctl.h --- a/master/ioctl.h Mon Dec 14 13:11:36 2009 +0100 +++ b/master/ioctl.h Mon Dec 14 13:25:50 2009 +0100 @@ -123,6 +123,7 @@ #define EC_IOCTL_VOE_WRITE EC_IOWR(0x3f, ec_ioctl_voe_t) #define EC_IOCTL_VOE_EXEC EC_IOWR(0x40, ec_ioctl_voe_t) #define EC_IOCTL_VOE_DATA EC_IOWR(0x41, ec_ioctl_voe_t) +#define EC_IOCTL_SET_MAX_CYCLE_SIZE EC_IOW(0x42, size_t) /*****************************************************************************/ diff -r e51cf2af3ff9 -r eb9185dfa8ac master/master.c --- a/master/master.c Mon Dec 14 13:11:36 2009 +0100 +++ b/master/master.c Mon Dec 14 13:25:50 2009 +0100 @@ -59,13 +59,14 @@ /** Frame timeout in cycles. */ static cycles_t timeout_cycles; - +static cycles_t sdo_injection_timeout_cycles; #else /** Frame timeout in jiffies. */ static unsigned long timeout_jiffies; - +static unsigned long sdo_injection_timeout_jiffies; + #endif /*****************************************************************************/ @@ -87,9 +88,11 @@ { #ifdef EC_HAVE_CYCLES timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); + sdo_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000); #else // one jiffy may always elapse between time measurement timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1); + sdo_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1); #endif } @@ -151,6 +154,9 @@ INIT_LIST_HEAD(&master->ext_datagram_queue); sema_init(&master->ext_queue_sem, 1); + INIT_LIST_HEAD(&master->sdo_datagram_queue); + master->max_queue_size = EC_MAX_DATA_SIZE; + INIT_LIST_HEAD(&master->domains); master->debug_level = debug_level; @@ -684,6 +690,91 @@ master->phase = EC_IDLE; } + +/*****************************************************************************/ + +/** Injects sdo datagrams that fit into the datagram queue + */ +void ec_master_inject_sdo_datagrams( + ec_master_t *master /**< EtherCAT master */ + ) +{ + ec_datagram_t *datagram, *n; + size_t queue_size = 0; + list_for_each_entry(datagram, &master->datagram_queue, queue) { + queue_size += datagram->data_size; + } + list_for_each_entry_safe(datagram, n, &master->sdo_datagram_queue, queue) { + queue_size += datagram->data_size; + if (queue_size <= master->max_queue_size) { + list_del_init(&datagram->queue); + if (master->debug_level) { + EC_DBG("Injecting SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); + } +#ifdef EC_HAVE_CYCLES + datagram->cycles_sent = 0; +#endif + datagram->jiffies_sent = 0; + ec_master_queue_datagram(master, datagram); + } + else { + if (datagram->data_size > master->max_queue_size) { + list_del_init(&datagram->queue); + datagram->state = EC_DATAGRAM_ERROR; + EC_ERR("SDO datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); + } + else { +#ifdef EC_HAVE_CYCLES + cycles_t cycles_now = get_cycles(); + if (cycles_now - datagram->cycles_sent + > sdo_injection_timeout_cycles) { +#else + if (jiffies - datagram->jiffies_sent + > sdo_injection_timeout_jiffies) { +#endif + unsigned int time_us; + list_del_init(&datagram->queue); + datagram->state = EC_DATAGRAM_ERROR; +#ifdef EC_HAVE_CYCLES + time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; +#else + time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); +#endif + EC_ERR("Timeout %u us: injecting SDO datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); + } + else { + if (master->debug_level) { + EC_DBG("Deferred injecting of SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); + } + } + } + } + } +} + +/*****************************************************************************/ + +/** Places a sdo datagram in the sdo datagram queue. + */ +void ec_master_queue_sdo_datagram( + ec_master_t *master, /**< EtherCAT master */ + ec_datagram_t *datagram /**< datagram */ + ) +{ + if (master->debug_level) { + EC_DBG("Requesting SDO datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); + } + datagram->state = EC_DATAGRAM_QUEUED; +#ifdef EC_HAVE_CYCLES + datagram->cycles_sent = get_cycles(); +#endif + datagram->jiffies_sent = jiffies; + + down(&master->io_sem); + list_add_tail(&datagram->queue, &master->sdo_datagram_queue); + up(&master->io_sem); +} + /*****************************************************************************/ /** Places a datagram in the datagram queue. @@ -695,6 +786,8 @@ { ec_datagram_t *queued_datagram; + if (datagram->state == EC_DATAGRAM_SENT) + return; // check, if the datagram is already queued list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { if (queued_datagram == datagram) { @@ -1001,7 +1094,8 @@ static int ec_master_idle_thread(void *priv_data) { ec_master_t *master = (ec_master_t *) priv_data; - + ec_slave_t *slave = NULL; + int fsm_exec; if (master->debug_level) EC_DBG("Idle thread running.\n"); @@ -1013,22 +1107,27 @@ ecrt_master_receive(master); up(&master->io_sem); - if (master->fsm_datagram.state == EC_DATAGRAM_SENT) - goto schedule; - - // execute master state machine + fsm_exec = 0; + // execute master & slave state machines if (down_interruptible(&master->master_sem)) break; - ec_fsm_master_exec(&master->fsm); + fsm_exec = ec_fsm_master_exec(&master->fsm); + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + ec_fsm_slave_exec(&slave->fsm); + } up(&master->master_sem); // queue and send down(&master->io_sem); - ec_master_queue_datagram(master, &master->fsm_datagram); + if (fsm_exec) { + ec_master_queue_datagram(master, &master->fsm_datagram); + } + ec_master_inject_sdo_datagrams(master); ecrt_master_send(master); up(&master->io_sem); - -schedule: + if (ec_fsm_master_idle(&master->fsm)) { set_current_state(TASK_INTERRUPTIBLE); schedule_timeout(1); @@ -1045,35 +1144,38 @@ /*****************************************************************************/ -/** Master kernel thread function for IDLE phase. +/** Master kernel thread function for OPERATION phase. */ static int ec_master_operation_thread(void *priv_data) { ec_master_t *master = (ec_master_t *) priv_data; - + ec_slave_t *slave = NULL; + int fsm_exec; if (master->debug_level) EC_DBG("Operation thread running.\n"); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); - if (master->injection_seq_rt != master->injection_seq_fsm || - master->fsm_datagram.state == EC_DATAGRAM_SENT || - master->fsm_datagram.state == EC_DATAGRAM_QUEUED) - goto schedule; - - // output statistics - ec_master_output_stats(master); - - // execute master state machine - if (down_interruptible(&master->master_sem)) - break; - ec_fsm_master_exec(&master->fsm); - up(&master->master_sem); - - // inject datagram - master->injection_seq_fsm++; - -schedule: + if (master->injection_seq_rt == master->injection_seq_fsm) { + // output statistics + ec_master_output_stats(master); + + fsm_exec = 0; + // execute master & slave state machines + if (down_interruptible(&master->master_sem)) + break; + fsm_exec += ec_fsm_master_exec(&master->fsm); + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + ec_fsm_slave_exec(&slave->fsm); + } + up(&master->master_sem); + + // inject datagrams (let the rt thread queue them, see ecrt_master_send) + if (fsm_exec) + master->injection_seq_fsm++; + } if (ec_fsm_master_idle(&master->fsm)) { set_current_state(TASK_INTERRUPTIBLE); schedule_timeout(1); @@ -1797,10 +1899,11 @@ ec_datagram_t *datagram, *n; if (master->injection_seq_rt != master->injection_seq_fsm) { - // inject datagram produced by master FSM + // inject datagrams produced by master & slave FSMs ec_master_queue_datagram(master, &master->fsm_datagram); master->injection_seq_rt = master->injection_seq_fsm; } + ec_master_inject_sdo_datagrams(master); if (unlikely(!master->main_device.link_state)) { // link is down, no datagram can be sent diff -r e51cf2af3ff9 -r eb9185dfa8ac master/master.h --- a/master/master.h Mon Dec 14 13:11:36 2009 +0100 +++ b/master/master.h Mon Dec 14 13:25:50 2009 +0100 @@ -114,6 +114,7 @@ unsigned int injection_seq_rt; /**< Datagram injection sequence number for the realtime side. */ + ec_slave_t *slaves; /**< Array of slaves on the bus. */ unsigned int slave_count; /**< Number of slaves on the bus. */ @@ -153,6 +154,8 @@ struct semaphore ext_queue_sem; /**< Semaphore protecting the \a ext_datagram_queue. */ + struct list_head sdo_datagram_queue; /**< SDO Datagram queue. */ + size_t max_queue_size; /** max. size of datagram queue */ struct list_head domains; /**< List of domains. */ unsigned int debug_level; /**< Master debug level. */ @@ -220,6 +223,8 @@ void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t); void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *); void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *); +void ec_master_queue_sdo_datagram(ec_master_t *, ec_datagram_t *); +void ec_master_inject_sdo_datagrams(ec_master_t *); // misc. void ec_master_attach_slave_configs(ec_master_t *); diff -r e51cf2af3ff9 -r eb9185dfa8ac master/module.c --- a/master/module.c Mon Dec 14 13:11:36 2009 +0100 +++ b/master/module.c Mon Dec 14 13:25:50 2009 +0100 @@ -318,6 +318,8 @@ /*****************************************************************************/ /** Outputs frame contents for debugging purposes. + * If the data block is larger than 256 bytes, only the first 128 + * and the last 128 bytes will be shown */ void ec_print_data(const uint8_t *data, /**< pointer to data */ size_t size /**< number of bytes to output */ @@ -332,6 +334,12 @@ printk("\n"); EC_DBG(""); } + if (i+1 == 128 && size > 256) + { + printk("dropped %d bytes\n",size-128-i); + i = size - 128; + EC_DBG(""); + } } printk("\n"); } diff -r e51cf2af3ff9 -r eb9185dfa8ac master/slave.c --- a/master/slave.c Mon Dec 14 13:11:36 2009 +0100 +++ b/master/slave.c Mon Dec 14 13:25:50 2009 +0100 @@ -67,6 +67,7 @@ ) { unsigned int i; + int ret; slave->master = master; slave->ring_position = ring_position; @@ -147,6 +148,20 @@ slave->sdo_dictionary_fetched = 0; slave->jiffies_preop = 0; + + // init state machine datagram + ec_datagram_init(&slave->fsm_datagram); + snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "slave%u-fsm",slave->ring_position); + ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE); + if (ret < 0) { + ec_datagram_clear(&slave->fsm_datagram); + EC_ERR("Failed to allocate Slave %u FSM datagram.\n",slave->ring_position); + return; + } + + // create state machine object + ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram); + } /*****************************************************************************/ @@ -191,6 +206,9 @@ if (slave->sii_words) kfree(slave->sii_words); + ec_fsm_slave_clear(&slave->fsm); + ec_datagram_clear(&slave->fsm_datagram); + } /*****************************************************************************/ diff -r e51cf2af3ff9 -r eb9185dfa8ac master/slave.h --- a/master/slave.h Mon Dec 14 13:11:36 2009 +0100 +++ b/master/slave.h Mon Dec 14 13:25:50 2009 +0100 @@ -45,6 +45,7 @@ #include "pdo.h" #include "sync.h" #include "sdo.h" +#include "fsm_slave.h" /*****************************************************************************/ @@ -157,6 +158,9 @@ struct list_head sdo_dictionary; /**< SDO dictionary list */ uint8_t sdo_dictionary_fetched; /**< Dictionary has been fetched. */ unsigned long jiffies_preop; /**< Time, the slave went to PREOP. */ + + ec_fsm_slave_t fsm; /**< Slave state machine. */ + ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */ }; /*****************************************************************************/