--- a/include/ecrt.h Mon Dec 14 13:29:06 2009 +0100
+++ b/include/ecrt.h Sun Dec 27 16:58:19 2009 +0100
@@ -699,12 +699,12 @@
);
-/** Set max. number of databytes in a cycle
- *
- */
-int ecrt_master_set_max_cycle_size(
+/** Set interval between calls to ecrt_master_send
+ *
+ */
+int ecrt_master_set_send_interval(
ec_master_t *master, /**< EtherCAT master. */
- size_t max_cycle_data_size /**< Max. number of databytes in a cycle */
+ size_t send_interval /**< Send interval in us */
);
/** Sends all datagrams in the queue.
--- a/lib/master.c Mon Dec 14 13:29:06 2009 +0100
+++ b/lib/master.c Sun Dec 27 16:58:19 2009 +0100
@@ -347,11 +347,11 @@
/*****************************************************************************/
-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",
+int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us)
+{
+ if (ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL,
+ &send_interval_us) == -1) {
+ fprintf(stderr, "Failed to set send interval: %s\n",
strerror(errno));
return -1; // FIXME
}
--- a/master/cdev.c Mon Dec 14 13:29:06 2009 +0100
+++ b/master/cdev.c Sun Dec 27 16:58:19 2009 +0100
@@ -808,12 +808,12 @@
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);
+ list_add_tail(&request.list, &request.slave->slave_sdo_requests);
up(&master->master_sem);
// wait for processing through FSM
- if (wait_event_interruptible(master->sdo_queue,
+ if (wait_event_interruptible(request.slave->sdo_queue,
request.req.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
@@ -828,7 +828,7 @@
}
// wait until master FSM has finished processing
- wait_event(master->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY);
+ wait_event(request.slave->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);
@@ -914,12 +914,12 @@
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);
+ list_add_tail(&request.list, &request.slave->slave_sdo_requests);
up(&master->master_sem);
// wait for processing through FSM
- if (wait_event_interruptible(master->sdo_queue,
+ if (wait_event_interruptible(request.slave->sdo_queue,
request.req.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
@@ -934,7 +934,7 @@
}
// wait until master FSM has finished processing
- wait_event(master->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY);
+ wait_event(request.slave->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);
@@ -1678,21 +1678,21 @@
/** 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;
+int ec_cdev_ioctl_set_send_interval(
+ ec_master_t *master, /**< EtherCAT master. */
+ unsigned long arg, /**< ioctl() argument. */
+ ec_cdev_priv_t *priv /**< Private data structure of file handle. */
+ )
+{
+ size_t send_interval;
+
+ if (copy_from_user(&send_interval, (void __user *) arg, sizeof(send_interval))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+ ec_master_set_send_interval(master,send_interval);
up(&master->master_sem);
return 0;
@@ -3057,7 +3057,7 @@
}
// schedule request.
- list_add_tail(&request.list, &master->foe_requests);
+ list_add_tail(&request.list, &request.slave->foe_requests);
up(&master->master_sem);
@@ -3067,7 +3067,7 @@
}
// wait for processing through FSM
- if (wait_event_interruptible(master->foe_queue,
+ if (wait_event_interruptible(request.slave->foe_queue,
request.req.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
@@ -3082,7 +3082,7 @@
}
// wait until master FSM has finished processing
- wait_event(master->foe_queue, request.req.state != EC_INT_REQUEST_BUSY);
+ wait_event(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_BUSY);
data.result = request.req.result;
data.error_code = request.req.error_code;
@@ -3172,12 +3172,12 @@
}
// schedule FoE write request.
- list_add_tail(&request.list, &master->foe_requests);
+ list_add_tail(&request.list, &request.slave->foe_requests);
up(&master->master_sem);
// wait for processing through FSM
- if (wait_event_interruptible(master->foe_queue,
+ if (wait_event_interruptible(request.slave->foe_queue,
request.req.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
@@ -3192,7 +3192,7 @@
}
// wait until master FSM has finished processing
- wait_event(master->foe_queue, request.req.state != EC_INT_REQUEST_BUSY);
+ wait_event(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_BUSY);
data.result = request.req.result;
data.error_code = request.req.error_code;
@@ -3492,10 +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);
+ case EC_IOCTL_SET_SEND_INTERVAL:
+ if (!(filp->f_mode & FMODE_WRITE))
+ return -EPERM;
+ return ec_cdev_ioctl_set_send_interval(master,arg,priv);
default:
return -ENOTTY;
}
--- a/master/fsm_master.c Mon Dec 14 13:29:06 2009 +0100
+++ b/master/fsm_master.c Sun Dec 27 16:58:19 2009 +0100
@@ -58,7 +58,6 @@
void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);
void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);
void ec_fsm_master_state_reg_request(ec_fsm_master_t *);
-void ec_fsm_master_state_foe_request(ec_fsm_master_t *);
/*****************************************************************************/
@@ -82,7 +81,6 @@
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
- ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);
ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,
@@ -102,7 +100,6 @@
{
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
- ec_fsm_foe_clear(&fsm->fsm_foe);
ec_fsm_pdo_clear(&fsm->fsm_pdo);
ec_fsm_change_clear(&fsm->fsm_change);
ec_fsm_slave_config_clear(&fsm->fsm_slave_config);
@@ -456,70 +453,32 @@
return 0;
}
-/*****************************************************************************/
-
-/** Check for pending FoE requests and process one.
- *
- * \return non-zero, if an FoE request is processed.
- */
-int ec_fsm_master_action_process_foe(
+
+/*****************************************************************************/
+
+/** Master action: IDLE.
+ *
+ * Does secondary work.
+ */
+void ec_fsm_master_action_idle(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave;
- ec_master_foe_request_t *request;
-
- // search the first request to be processed
- while (1) {
- if (list_empty(&master->foe_requests))
- break;
-
- // get first request
- request = list_entry(master->foe_requests.next,
- ec_master_foe_request_t, list);
- list_del_init(&request->list); // dequeue
- request->req.state = EC_INT_REQUEST_BUSY;
- slave = request->slave;
-
- if (master->debug_level)
- EC_DBG("Processing FoE request for slave %u.\n",
- slave->ring_position);
-
- fsm->foe_request = &request->req;
- fsm->slave = slave;
- fsm->state = ec_fsm_master_state_foe_request;
- fsm->idle = 0;
- ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
- ec_fsm_foe_exec(&fsm->fsm_foe);
- return 1;
- }
-
- return 0;
-}
-
-/*****************************************************************************/
-
-/** Master action: IDLE.
- *
- * Does secondary work.
- */
-void ec_fsm_master_action_idle(
- ec_fsm_master_t *fsm /**< Master state machine. */
- )
-{
- ec_master_t *master = fsm->master;
- ec_slave_t *slave;
-
- // Check for pending SDO requests
+
+ // Check for pending internal SDO requests
if (ec_fsm_master_action_process_sdo(fsm))
return;
- // Check for pending FoE requests
- if (ec_fsm_master_action_process_foe(fsm))
- return;
-
- // check, if slaves have an SDO dictionary to read out.
+ // enable processing of SDO/FOE requests
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_fsm_slave_ready(&slave->fsm);
+ }
+
+ // check, if slaves have an SDO dictionary to read out.
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
@@ -932,43 +891,6 @@
/*****************************************************************************/
-/** Master state: WRITE FOE.
- */
-void ec_fsm_master_state_foe_request(
- ec_fsm_master_t *fsm /**< Master state machine. */
- )
-{
- ec_master_t *master = fsm->master;
- ec_foe_request_t *request = fsm->foe_request;
- ec_slave_t *slave = fsm->slave;
-
- if (ec_fsm_foe_exec(&fsm->fsm_foe))
- return;
-
- fsm->idle = 1;
-
- if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
- EC_ERR("Failed to handle FoE request to slave %u.\n",
- slave->ring_position);
- request->state = EC_INT_REQUEST_FAILURE;
- wake_up(&master->foe_queue);
- ec_fsm_master_restart(fsm);
- return;
- }
-
- // finished transferring FoE
- if (master->debug_level)
- EC_DBG("Successfully transferred %zu bytes of FoE data from/to"
- " slave %u.\n", request->data_size, slave->ring_position);
-
- request->state = EC_INT_REQUEST_SUCCESS;
- wake_up(&master->foe_queue);
-
- ec_fsm_master_restart(fsm);
-}
-
-/*****************************************************************************/
-
/** Master state: SDO DICTIONARY.
*/
void ec_fsm_master_state_sdo_dictionary(
@@ -1017,14 +939,14 @@
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);
+ wake_up(&fsm->slave->sdo_queue);
ec_fsm_master_restart(fsm);
return;
}
// SDO request finished
request->state = EC_INT_REQUEST_SUCCESS;
- wake_up(&master->sdo_queue);
+ wake_up(&fsm->slave->sdo_queue);
if (master->debug_level)
EC_DBG("Finished internal SDO request for slave %u.\n",
--- a/master/fsm_master.h Mon Dec 14 13:29:06 2009 +0100
+++ b/master/fsm_master.h Sun Dec 27 16:58:19 2009 +0100
@@ -116,8 +116,6 @@
off_t sii_index; /**< index to SII write request data */
ec_sdo_request_t *sdo_request; /**< SDO request to process. */
ec_reg_request_t *reg_request; /**< Register request to process. */
- ec_foe_request_t *foe_request; /**< FoE request to process. */
- off_t foe_index; /**< index to FoE write request data */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
ec_fsm_pdo_t fsm_pdo; /**< PDO configuration state machine. */
@@ -125,7 +123,6 @@
ec_fsm_slave_config_t fsm_slave_config; /**< slave state machine */
ec_fsm_slave_scan_t fsm_slave_scan; /**< slave state machine */
ec_fsm_sii_t fsm_sii; /**< SII state machine */
- ec_fsm_foe_t fsm_foe; /**< FoE state machine */
};
/*****************************************************************************/
--- a/master/fsm_sii.c Mon Dec 14 13:29:06 2009 +0100
+++ b/master/fsm_sii.c Sun Dec 27 16:58:19 2009 +0100
@@ -301,7 +301,6 @@
}
// issue check/fetch datagram again
- fsm->datagram->state = EC_DATAGRAM_INIT;
fsm->retries = EC_FSM_RETRIES;
return;
}
@@ -435,7 +434,6 @@
#endif
// issue check datagram again
fsm->retries = EC_FSM_RETRIES;
- fsm->datagram->state = EC_DATAGRAM_INIT;
return;
}
@@ -454,7 +452,6 @@
// issue check datagram again
fsm->retries = EC_FSM_RETRIES;
- fsm->datagram->state = EC_DATAGRAM_INIT;
return;
}
--- a/master/fsm_slave.c Mon Dec 14 13:29:06 2009 +0100
+++ b/master/fsm_slave.c Sun Dec 27 16:58:19 2009 +0100
@@ -42,7 +42,11 @@
/*****************************************************************************/
void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
+void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
+int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
+int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
/*****************************************************************************/
@@ -58,10 +62,13 @@
fsm->slave = slave;
fsm->datagram = datagram;
fsm->datagram->data_size = 0;
- fsm->state = ec_fsm_slave_state_idle;
+ if (slave->master->debug_level)
+ EC_DBG("init fsm for slave %u...\n",slave->ring_position);
+ fsm->state = ec_fsm_slave_state_idle;
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
+ ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
}
/*****************************************************************************/
@@ -74,6 +81,7 @@
{
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
+ ec_fsm_foe_clear(&fsm->fsm_foe);
}
/*****************************************************************************/
@@ -97,16 +105,70 @@
return;
}
+
+/*****************************************************************************/
+
+/** Sets the current state of the state machine to READY
+ *
+ */
+void ec_fsm_slave_ready(
+ ec_fsm_slave_t *fsm /**< Slave state machine. */
+ )
+{
+ if (fsm->state == ec_fsm_slave_state_idle) {
+ if (fsm->slave->master->debug_level) {
+ EC_DBG("Slave %u ready for SDO/FOE.\n",fsm->slave->ring_position);
+ }
+ fsm->state = ec_fsm_slave_state_ready;
+ }
+ 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. */
+ )
+{
+ // do nothing
+}
+
+
+/*****************************************************************************/
+
+/** Slave state: READY.
+ *
+ *
+ */
+void ec_fsm_slave_state_ready(
+ ec_fsm_slave_t *fsm /**< Slave state machine. */
+ )
+{
+ // Check for pending external SDO requests
+ if (ec_fsm_slave_action_process_sdo(fsm))
+ return;
+ // Check for pending FOE requests
+ if (ec_fsm_slave_action_process_foe(fsm))
+ return;
+
+}
+
+
+/*****************************************************************************/
+
+/** Check for pending SDO requests and process one.
+ *
+ * \return non-zero, if an SDO request is processed.
+ */
+int ec_fsm_slave_action_process_sdo(
ec_fsm_slave_t *fsm /**< Slave state machine. */
)
{
@@ -114,20 +176,29 @@
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;
+ // search the first external request to be processed
+ list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
+
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;
- }
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_WARN("Aborting SDO request, slave %u has ERROR.\n",
+ slave->ring_position);
+ request->req.state = EC_INT_REQUEST_FAILURE;
+ wake_up(&slave->sdo_queue);
+ fsm->sdo_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+ if (slave->current_state == EC_SLAVE_STATE_INIT) {
+ EC_WARN("Aborting SDO request, slave %u is in INIT.\n",
+ slave->ring_position);
+ request->req.state = EC_INT_REQUEST_FAILURE;
+ wake_up(&slave->sdo_queue);
+ fsm->sdo_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+ request->req.state = EC_INT_REQUEST_BUSY;
// Found pending SDO request. Execute it!
if (master->debug_level)
@@ -139,10 +210,56 @@
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;
- }
-}
+ ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
+ return 1;
+ }
+ return 0;
+}
+
+
+/*****************************************************************************/
+
+/** Check for pending FOE requests and process one.
+ *
+ * \return non-zero, if an FOE request is processed.
+ */
+int ec_fsm_slave_action_process_foe(
+ ec_fsm_slave_t *fsm /**< Slave state machine. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_master_foe_request_t *request, *next;
+
+ // search the first request to be processed
+ list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_WARN("Aborting FOE request, slave %u has ERROR.\n",
+ slave->ring_position);
+ request->req.state = EC_INT_REQUEST_FAILURE;
+ wake_up(&slave->sdo_queue);
+ fsm->sdo_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+ list_del_init(&request->list); // dequeue
+ request->req.state = EC_INT_REQUEST_BUSY;
+
+ if (master->debug_level)
+ EC_DBG("Processing FOE request for slave %u.\n",
+ slave->ring_position);
+
+ fsm->foe_request = &request->req;
+ fsm->state = ec_fsm_slave_state_foe_request;
+ ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
+ ec_fsm_foe_exec(&fsm->fsm_foe);
+ ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
+ return 1;
+ }
+ return 0;
+}
+
+
/*****************************************************************************/
@@ -158,28 +275,69 @@
if (ec_fsm_coe_exec(&fsm->fsm_coe))
{
- ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram);
+ ec_master_queue_external_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);
+ wake_up(&slave->foe_queue);
+ fsm->foe_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return;
+ }
if (master->debug_level)
EC_DBG("Finished SDO request for slave %u.\n",
fsm->slave->ring_position);
+ // SDO request finished
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up(&slave->sdo_queue);
+
fsm->sdo_request = NULL;
- fsm->datagram->data_size = 0;
- fsm->state = ec_fsm_slave_state_idle;
-}
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
+
+/*****************************************************************************/
+
+/** Slave state: FOE REQUEST.
+ */
+void ec_fsm_slave_state_foe_request(
+ ec_fsm_slave_t *fsm /**< Slave state machine. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_foe_request_t *request = fsm->foe_request;
+
+ if (ec_fsm_foe_exec(&fsm->fsm_foe))
+ {
+ ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
+ return;
+ }
+
+ if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
+ EC_ERR("Failed to handle FoE request to slave %u.\n",
+ slave->ring_position);
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up(&slave->foe_queue);
+ fsm->foe_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return;
+ }
+
+ // finished transferring FoE
+ if (master->debug_level)
+ EC_DBG("Successfully transferred %u bytes of FoE data from/to"
+ " slave %u.\n", request->data_size, slave->ring_position);
+
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up(&slave->foe_queue);
+
+ fsm->foe_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
--- a/master/fsm_slave.h Mon Dec 14 13:29:06 2009 +0100
+++ b/master/fsm_slave.h Sun Dec 27 16:58:19 2009 +0100
@@ -40,7 +40,7 @@
#include "datagram.h"
#include "sdo_request.h"
#include "fsm_coe.h"
-
+#include "fsm_foe.h"
typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */
@@ -52,8 +52,11 @@
void (*state)(ec_fsm_slave_t *); /**< master state function */
ec_sdo_request_t *sdo_request; /**< SDO request to process. */
+ ec_foe_request_t *foe_request; /**< FoE request to process. */
+ off_t foe_index; /**< index to FoE write request data */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
+ ec_fsm_foe_t fsm_foe; /**< FoE state machine */
};
/*****************************************************************************/
@@ -62,7 +65,7 @@
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 *);
+void ec_fsm_slave_ready(ec_fsm_slave_t *);
/*****************************************************************************/
--- a/master/globals.h Mon Dec 14 13:29:06 2009 +0100
+++ b/master/globals.h Sun Dec 27 16:58:19 2009 +0100
@@ -52,7 +52,7 @@
#define EC_SDO_INJECTION_TIMEOUT 10000
/** time to send a byte in nanoseconds. */
-#define EC_BYTE_TRANSMITION_TIME 80LL
+#define EC_BYTE_TRANSMITION_TIME 80
/** Number of state machine retries on datagram timeout. */
#define EC_FSM_RETRIES 3
--- a/master/ioctl.h Mon Dec 14 13:29:06 2009 +0100
+++ b/master/ioctl.h Sun Dec 27 16:58:19 2009 +0100
@@ -123,7 +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)
+#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x42, size_t)
/*****************************************************************************/
--- a/master/master.c Mon Dec 14 13:29:06 2009 +0100
+++ b/master/master.c Sun Dec 27 16:58:19 2009 +0100
@@ -41,7 +41,7 @@
#include <linux/delay.h>
#include <linux/device.h>
#include <linux/version.h>
-
+#include <linux/hrtimer.h>
#include "globals.h"
#include "slave.h"
#include "slave_config.h"
@@ -54,6 +54,10 @@
/*****************************************************************************/
+/** Set to 1 to enable external datagram injection debugging.
+ */
+#define DEBUG_INJECT 0
+
#ifdef EC_HAVE_CYCLES
/** Frame timeout in cycles.
@@ -154,8 +158,8 @@
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->external_datagram_queue);
+ ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase
INIT_LIST_HEAD(&master->domains);
@@ -184,15 +188,9 @@
INIT_LIST_HEAD(&master->sii_requests);
init_waitqueue_head(&master->sii_queue);
- INIT_LIST_HEAD(&master->slave_sdo_requests);
- init_waitqueue_head(&master->sdo_queue);
-
INIT_LIST_HEAD(&master->reg_requests);
init_waitqueue_head(&master->reg_queue);
- INIT_LIST_HEAD(&master->foe_requests);
- init_waitqueue_head(&master->foe_queue);
-
// init devices
ret = ec_device_init(&master->main_device, master);
if (ret < 0)
@@ -405,39 +403,37 @@
wake_up(&master->reg_queue);
}
- // SDO requests
- while (1) {
- ec_master_sdo_request_t *request;
- 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
- EC_INFO("Discarding SDO request, slave %u does not exist anymore.\n",
- request->slave->ring_position);
- request->req.state = EC_INT_REQUEST_FAILURE;
- wake_up(&master->sdo_queue);
- }
-
- // FoE requests
- while (1) {
- ec_master_foe_request_t *request;
- if (list_empty(&master->foe_requests))
- break;
- // get first request
- request = list_entry(master->foe_requests.next,
- ec_master_foe_request_t, list);
- list_del_init(&request->list); // dequeue
- EC_INFO("Discarding FOE request, slave %u does not exist anymore.\n",
- request->slave->ring_position);
- request->req.state = EC_INT_REQUEST_FAILURE;
- wake_up(&master->foe_queue);
- }
-
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
+ // SDO requests
+ while (1) {
+ ec_master_sdo_request_t *request;
+ if (list_empty(&slave->slave_sdo_requests))
+ break;
+ // get first request
+ request = list_entry(slave->slave_sdo_requests.next,
+ ec_master_sdo_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_INFO("Discarding SDO request, slave %u does not exist anymore.\n",
+ slave->ring_position);
+ request->req.state = EC_INT_REQUEST_FAILURE;
+ wake_up(&slave->sdo_queue);
+ }
+ // FoE requests
+ while (1) {
+ ec_master_foe_request_t *request;
+ if (list_empty(&slave->foe_requests))
+ break;
+ // get first request
+ request = list_entry(slave->foe_requests.next,
+ ec_master_foe_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_INFO("Discarding FOE request, slave %u does not exist anymore.\n",
+ slave->ring_position);
+ request->req.state = EC_INT_REQUEST_FAILURE;
+ wake_up(&slave->foe_queue);
+ }
ec_slave_clear(slave);
}
@@ -693,86 +689,118 @@
/*****************************************************************************/
-/** 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);
- }
+/** Injects external datagrams that fit into the datagram queue
+ */
+void ec_master_inject_external_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->external_datagram_queue, queue) {
+ queue_size += datagram->data_size;
+ if (queue_size <= master->max_queue_size) {
+ list_del_init(&datagram->queue);
+#if DEBUG_INJECT
+ if (master->debug_level) {
+ EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
+ }
+#endif
#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 {
+ 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("External 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) {
+ 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;
+ 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;
+ 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(
+ time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
+#endif
+ EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size);
+ }
+ else {
+#if DEBUG_INJECT
+ if (master->debug_level) {
+ EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
+ }
+#endif
+ }
+ }
+ }
+ }
+}
+
+/*****************************************************************************/
+
+/** sets the expected interval between calls to ecrt_master_send
+ and calculates the maximum amount of data to queue
+ */
+void ec_master_set_send_interval(
+ ec_master_t *master, /**< EtherCAT master */
+ size_t send_interval /**< send interval */
+ )
+{
+ master->send_interval = send_interval;
+ master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME;
+ master->max_queue_size -= master->max_queue_size / 10;
+}
+
+
+/*****************************************************************************/
+
+/** Places an external datagram in the sdo datagram queue.
+ */
+void ec_master_queue_external_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;
+ ec_datagram_t *queued_datagram;
+
+ down(&master->io_sem);
+ // check, if the datagram is already queued
+ list_for_each_entry(queued_datagram, &master->external_datagram_queue, queue) {
+ if (queued_datagram == datagram) {
+ datagram->state = EC_DATAGRAM_QUEUED;
+ return;
+ }
+ }
+#if DEBUG_INJECT
+ if (master->debug_level) {
+ EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size);
+ }
+#endif
+ list_add_tail(&datagram->queue, &master->external_datagram_queue);
+ 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);
+ datagram->cycles_sent = get_cycles();
+#endif
+ datagram->jiffies_sent = jiffies;
+
+ master->fsm.idle = 0;
+ up(&master->io_sem);
}
/*****************************************************************************/
@@ -821,12 +849,11 @@
/** Sends the datagrams in the queue.
*
- * \return 0 in case of success, else < 0
*/
void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
{
ec_datagram_t *datagram, *next;
- size_t datagram_size;
+ size_t datagram_size;
uint8_t *frame_data, *cur_data;
void *follows_word;
#ifdef EC_HAVE_CYCLES
@@ -1087,6 +1114,69 @@
}
}
+
+/*****************************************************************************/
+/*
+ * Sleep related functions:
+ */
+static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer)
+{
+ struct hrtimer_sleeper *t =
+ container_of(timer, struct hrtimer_sleeper, timer);
+ struct task_struct *task = t->task;
+
+ t->task = NULL;
+ if (task)
+ wake_up_process(task);
+
+ return HRTIMER_NORESTART;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+/* compatibility with new hrtimer interface */
+static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer)
+{
+ return timer->expires;
+}
+
+static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time)
+{
+ timer->expires = time;
+}
+#endif
+
+
+void ec_master_nanosleep(const unsigned long nsecs)
+{
+ struct hrtimer_sleeper t;
+ enum hrtimer_mode mode = HRTIMER_MODE_REL;
+ hrtimer_init(&t.timer, CLOCK_MONOTONIC,mode);
+ t.timer.function = ec_master_nanosleep_wakeup;
+ t.task = current;
+#ifdef CONFIG_HIGH_RES_TIMERS
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24)
+ t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART;
+#elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26)
+ t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ;
+#elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28)
+ t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED;
+#endif
+#endif
+ hrtimer_set_expires(&t.timer, ktime_set(0,nsecs));
+ do {
+ set_current_state(TASK_INTERRUPTIBLE);
+ hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode);
+
+ if (likely(t.task))
+ schedule();
+
+ hrtimer_cancel(&t.timer);
+ mode = HRTIMER_MODE_ABS;
+
+ } while (t.task && !signal_pending(current));
+}
+
+
/*****************************************************************************/
/** Master kernel thread function for IDLE phase.
@@ -1096,8 +1186,10 @@
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");
+ size_t sent_bytes;
+ ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase
+ if (master->debug_level)
+ EC_DBG("Idle thread running with send interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size);
while (!kthread_should_stop()) {
ec_datagram_output_stats(&master->fsm_datagram);
@@ -1124,17 +1216,15 @@
if (fsm_exec) {
ec_master_queue_datagram(master, &master->fsm_datagram);
}
- ec_master_inject_sdo_datagrams(master);
+ ec_master_inject_external_datagrams(master);
ecrt_master_send(master);
+ sent_bytes = master->main_device.tx_skb[master->main_device.tx_ring_index]->len;
up(&master->io_sem);
- if (ec_fsm_master_idle(&master->fsm)) {
- set_current_state(TASK_INTERRUPTIBLE);
- schedule_timeout(1);
- }
- else {
- schedule();
- }
+ if (ec_fsm_master_idle(&master->fsm))
+ ec_master_nanosleep(master->send_interval*1000);
+ else
+ ec_master_nanosleep(sent_bytes*EC_BYTE_TRANSMITION_TIME);
}
if (master->debug_level)
@@ -1152,7 +1242,7 @@
ec_slave_t *slave = NULL;
int fsm_exec;
if (master->debug_level)
- EC_DBG("Operation thread running.\n");
+ EC_DBG("Operation thread running with fsm interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size);
while (!kthread_should_stop()) {
ec_datagram_output_stats(&master->fsm_datagram);
@@ -1176,14 +1266,9 @@
if (fsm_exec)
master->injection_seq_fsm++;
}
- if (ec_fsm_master_idle(&master->fsm)) {
- set_current_state(TASK_INTERRUPTIBLE);
- schedule_timeout(1);
- }
- else {
- schedule();
- }
- }
+ // the op thread should not work faster than the sending RT thread
+ ec_master_nanosleep(master->send_interval*1000);
+ }
if (master->debug_level)
EC_DBG("Master OP thread exiting...\n");
@@ -1903,7 +1988,7 @@
ec_master_queue_datagram(master, &master->fsm_datagram);
master->injection_seq_rt = master->injection_seq_fsm;
}
- ec_master_inject_sdo_datagrams(master);
+ ec_master_inject_external_datagrams(master);
if (unlikely(!master->main_device.link_state)) {
// link is down, no datagram can be sent
@@ -1918,7 +2003,7 @@
}
// send frames
- ec_master_send_datagrams(master);
+ ec_master_send_datagrams(master);
}
/*****************************************************************************/
--- a/master/master.h Mon Dec 14 13:29:06 2009 +0100
+++ b/master/master.h Sun Dec 27 16:58:19 2009 +0100
@@ -154,7 +154,8 @@
struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
ext_datagram_queue. */
- struct list_head sdo_datagram_queue; /**< SDO Datagram queue. */
+ struct list_head external_datagram_queue; /**< External Datagram queue. */
+ size_t send_interval; /* interval between calls to ecrt_master_send */
size_t max_queue_size; /** max. size of datagram queue */
struct list_head domains; /**< List of domains. */
@@ -185,16 +186,9 @@
wait_queue_head_t sii_queue; /**< Wait queue for SII
write requests from user space. */
- struct list_head slave_sdo_requests; /**< SDO access requests. */
- wait_queue_head_t sdo_queue; /**< Wait queue for SDO access requests
- from user space. */
-
struct list_head reg_requests; /**< Register requests. */
wait_queue_head_t reg_queue; /**< Wait queue for register requests. */
- struct list_head foe_requests; /**< FoE write requests. */
- wait_queue_head_t foe_queue; /**< Wait queue for FoE
- write requests from user space. */
};
/*****************************************************************************/
@@ -223,10 +217,11 @@
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 *);
+void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
+void ec_master_inject_external_datagrams(ec_master_t *);
// misc.
+void ec_master_set_send_interval(ec_master_t *,size_t);
void ec_master_attach_slave_configs(ec_master_t *);
ec_slave_t *ec_master_find_slave(ec_master_t *, uint16_t, uint16_t);
const ec_slave_t *ec_master_find_slave_const(const ec_master_t *, uint16_t,
--- a/master/slave.c Mon Dec 14 13:29:06 2009 +0100
+++ b/master/slave.c Sun Dec 27 16:58:19 2009 +0100
@@ -149,6 +149,12 @@
slave->sdo_dictionary_fetched = 0;
slave->jiffies_preop = 0;
+ INIT_LIST_HEAD(&slave->slave_sdo_requests);
+ init_waitqueue_head(&slave->sdo_queue);
+
+ INIT_LIST_HEAD(&slave->foe_requests);
+ init_waitqueue_head(&slave->foe_queue);
+
// 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);
--- a/master/slave.h Mon Dec 14 13:29:06 2009 +0100
+++ b/master/slave.h Sun Dec 27 16:58:19 2009 +0100
@@ -159,6 +159,12 @@
uint8_t sdo_dictionary_fetched; /**< Dictionary has been fetched. */
unsigned long jiffies_preop; /**< Time, the slave went to PREOP. */
+ struct list_head slave_sdo_requests; /**< SDO access requests. */
+ wait_queue_head_t sdo_queue; /**< Wait queue for SDO access requests
+ from user space. */
+ struct list_head foe_requests; /**< FoE write requests. */
+ wait_queue_head_t foe_queue; /**< Wait queue for FoE
+ write requests from user space. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
};