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

Moved FOE request queue and fsm into slaves

This commit is contained in:
Martin Troxler 2009-12-01 16:00:22 +01:00
parent abb3588747
commit d823576945
9 changed files with 173 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
master/cdev.c
View File

@ -3057,7 +3057,7 @@ int ec_cdev_ioctl_slave_foe_read(
}
// 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 @@ int ec_cdev_ioctl_slave_foe_read(
}
// 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 @@ int ec_cdev_ioctl_slave_foe_read(
}
// 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 @@ int ec_cdev_ioctl_slave_foe_write(
}
// 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 @@ int ec_cdev_ioctl_slave_foe_write(
}
// 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;

87
master/fsm_master.c
View File

@ -58,7 +58,6 @@ void ec_fsm_master_state_write_sii(ec_fsm_master_t *);
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 @@ void ec_fsm_master_init(
// 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 @@ void ec_fsm_master_clear(
{
// 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,47 +453,6 @@ int ec_fsm_master_action_process_sdo(
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(
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;
}
/*****************************************************************************/
@ -511,14 +467,10 @@ void ec_fsm_master_action_idle(
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.
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
@ -932,43 +884,6 @@ void ec_fsm_master_state_write_sii(
/*****************************************************************************/
/** 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 %u 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(

3
master/fsm_master.h
View File

@ -116,8 +116,6 @@ struct ec_fsm_master {
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 @@ struct ec_fsm_master {
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 */
};
/*****************************************************************************/

124
master/fsm_slave.c
View File

@ -42,7 +42,10 @@
/*****************************************************************************/
void ec_fsm_slave_state_idle(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 *);
/*****************************************************************************/
@ -62,6 +65,7 @@ void ec_fsm_slave_init(
// init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
}
/*****************************************************************************/
@ -74,6 +78,7 @@ void ec_fsm_slave_clear(
{
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_fsm_foe_clear(&fsm->fsm_foe);
}
/*****************************************************************************/
@ -101,20 +106,41 @@ void ec_fsm_slave_exec(
* 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. */
)
{
// 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. */
)
{
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
// 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;
@ -137,11 +163,48 @@ void ec_fsm_slave_state_idle(
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) {
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;
}
/*****************************************************************************/
/** Slave state: SDO_REQUEST.
@ -156,7 +219,7 @@ void ec_fsm_slave_state_sdo_request(
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)) {
@ -169,15 +232,56 @@ void ec_fsm_slave_state_sdo_request(
return;
}
// SDO request finished
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&slave->sdo_queue);
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;
}
/*****************************************************************************/
/** 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_idle;
}

5
master/fsm_slave.h
View File

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

72
master/master.c
View File

@ -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,7 +158,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
INIT_LIST_HEAD(&master->ext_datagram_queue);
sema_init(&master->ext_queue_sem, 1);
INIT_LIST_HEAD(&master->sdo_datagram_queue);
INIT_LIST_HEAD(&master->external_datagram_queue);
master->max_queue_size = EC_MAX_DATA_SIZE;
INIT_LIST_HEAD(&master->domains);
@ -187,9 +191,6 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
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)
@ -402,21 +403,6 @@ void ec_master_clear_slaves(ec_master_t *master)
wake_up(&master->reg_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++) {
@ -430,10 +416,24 @@ void ec_master_clear_slaves(ec_master_t *master)
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);
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);
}
@ -689,9 +689,9 @@ void ec_master_leave_operation_phase(
/*****************************************************************************/
/** Injects sdo datagrams that fit into the datagram queue
/** Injects external datagrams that fit into the datagram queue
*/
void ec_master_inject_sdo_datagrams(
void ec_master_inject_external_datagrams(
ec_master_t *master /**< EtherCAT master */
)
{
@ -700,13 +700,15 @@ void ec_master_inject_sdo_datagrams(
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) {
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 SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
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
@ -717,7 +719,7 @@ void ec_master_inject_sdo_datagrams(
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);
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
@ -736,12 +738,14 @@ void ec_master_inject_sdo_datagrams(
#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);
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 SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
}
#endif
}
}
}
@ -750,16 +754,18 @@ void ec_master_inject_sdo_datagrams(
/*****************************************************************************/
/** Places a sdo datagram in the sdo datagram queue.
/** Places an external datagram in the sdo datagram queue.
*/
void ec_master_queue_sdo_datagram(
void ec_master_queue_external_datagram(
ec_master_t *master, /**< EtherCAT master */
ec_datagram_t *datagram /**< datagram */
)
{
#if DEBUG_INJECT
if (master->debug_level) {
EC_DBG("Requesting SDO datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size);
EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size);
}
#endif
datagram->state = EC_DATAGRAM_QUEUED;
#ifdef EC_HAVE_CYCLES
datagram->cycles_sent = get_cycles();
@ -769,7 +775,7 @@ void ec_master_queue_sdo_datagram(
master->fsm.idle = 0;
down(&master->io_sem);
list_add_tail(&datagram->queue, &master->sdo_datagram_queue);
list_add_tail(&datagram->queue, &master->external_datagram_queue);
up(&master->io_sem);
}
@ -1122,7 +1128,7 @@ static int ec_master_idle_thread(void *priv_data)
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);
up(&master->io_sem);
@ -1901,7 +1907,7 @@ void ecrt_master_send(ec_master_t *master)
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

9
master/master.h
View File

@ -154,7 +154,7 @@ struct ec_master {
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 max_queue_size; /** max. size of datagram queue */
struct list_head domains; /**< List of domains. */
@ -188,9 +188,6 @@ struct ec_master {
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. */
};
/*****************************************************************************/
@ -219,8 +216,8 @@ void ec_master_eoe_stop(ec_master_t *);
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_attach_slave_configs(ec_master_t *);

3
master/slave.c
View File

@ -152,6 +152,9 @@ void ec_slave_init(
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);

3
master/slave.h
View File

@ -162,6 +162,9 @@ struct ec_slave
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. */
};