Each slave got its own SDO state machine (faster handling of external SDO requests)
This commit is contained in:
parent
286a65516b
commit
0d17968348
|
|
@ -44,6 +44,7 @@ ec_master-objs := \
|
|||
fsm_coe.o \
|
||||
fsm_foe.o \
|
||||
fsm_master.o \
|
||||
fsm_slave.o \
|
||||
fsm_pdo.o \
|
||||
fsm_pdo_entry.o \
|
||||
fsm_sii.o \
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ noinst_HEADERS = \
|
|||
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 \
|
||||
|
|
|
|||
|
|
@ -805,6 +805,8 @@ int ec_cdev_ioctl_slave_sdo_upload(
|
|||
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);
|
||||
|
||||
|
|
@ -828,6 +830,9 @@ int ec_cdev_ioctl_slave_sdo_upload(
|
|||
// 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 @@ int ec_cdev_ioctl_slave_sdo_download(
|
|||
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);
|
||||
|
||||
|
|
@ -929,6 +936,9 @@ int ec_cdev_ioctl_slave_sdo_download(
|
|||
// 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;
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@ 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 *);
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Constructor.
|
||||
|
|
@ -115,18 +116,21 @@ void ec_fsm_master_clear(
|
|||
*
|
||||
* If the state machine's datagram is not sent or received yet, the execution
|
||||
* of the state machine is delayed to the next cycle.
|
||||
*
|
||||
* \return true, if the state machine was executed
|
||||
*/
|
||||
void ec_fsm_master_exec(
|
||||
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 @@ int ec_fsm_master_action_process_sdo(
|
|||
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 @@ int ec_fsm_master_action_process_sdo(
|
|||
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 @@ int ec_fsm_master_action_process_sdo(
|
|||
|
||||
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 @@ int ec_fsm_master_action_process_sdo(
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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 @@ void ec_fsm_master_state_sdo_request(
|
|||
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 @@ void ec_fsm_master_state_sdo_request(
|
|||
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
|
||||
|
|
|
|||
|
|
@ -133,7 +133,7 @@ struct ec_fsm_master {
|
|||
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 *);
|
||||
|
||||
/*****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -248,7 +248,6 @@ void ec_fsm_sii_state_read_check(
|
|||
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 @@ void ec_fsm_sii_state_read_fetch(
|
|||
}
|
||||
|
||||
// issue check/fetch datagram again
|
||||
fsm->datagram->state = EC_DATAGRAM_INIT;
|
||||
fsm->retries = EC_FSM_RETRIES;
|
||||
return;
|
||||
}
|
||||
|
|
@ -435,6 +435,7 @@ void ec_fsm_sii_state_write_check2(
|
|||
#endif
|
||||
// issue check datagram again
|
||||
fsm->retries = EC_FSM_RETRIES;
|
||||
fsm->datagram->state = EC_DATAGRAM_INIT;
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -453,6 +454,7 @@ void ec_fsm_sii_state_write_check2(
|
|||
|
||||
// issue check datagram again
|
||||
fsm->retries = EC_FSM_RETRIES;
|
||||
fsm->datagram->state = EC_DATAGRAM_INIT;
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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__
|
||||
103
master/master.c
103
master/master.c
|
|
@ -151,6 +151,8 @@ 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->domains);
|
||||
|
||||
master->debug_level = debug_level;
|
||||
|
|
@ -684,6 +686,39 @@ void ec_master_leave_operation_phase(
|
|||
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 *sdo_datagram, *n;
|
||||
list_for_each_entry_safe(sdo_datagram, n, &master->sdo_datagram_queue, queue) {
|
||||
list_del_init(&sdo_datagram->queue);
|
||||
if (master->debug_level)
|
||||
EC_DBG("queuing SDO datagram %08x\n",(unsigned int)sdo_datagram);
|
||||
ec_master_queue_datagram(master, sdo_datagram);
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** 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 */
|
||||
)
|
||||
{
|
||||
down(&master->io_sem);
|
||||
list_add_tail(&datagram->queue, &master->sdo_datagram_queue);
|
||||
datagram->state = EC_DATAGRAM_QUEUED;
|
||||
up(&master->io_sem);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Places a datagram in the datagram queue.
|
||||
|
|
@ -695,6 +730,8 @@ void ec_master_queue_datagram(
|
|||
{
|
||||
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 +1038,8 @@ void ec_master_output_stats(ec_master_t *master /**< EtherCAT master */)
|
|||
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 +1051,27 @@ static int ec_master_idle_thread(void *priv_data)
|
|||
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 +1088,38 @@ schedule:
|
|||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** 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;
|
||||
if (master->injection_seq_rt == master->injection_seq_fsm) {
|
||||
// output statistics
|
||||
ec_master_output_stats(master);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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:
|
||||
// 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 +1843,11 @@ void ecrt_master_send(ec_master_t *master)
|
|||
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
|
||||
|
|
|
|||
|
|
@ -114,6 +114,7 @@ struct ec_master {
|
|||
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 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 domains; /**< List of domains. */
|
||||
|
||||
unsigned int debug_level; /**< Master debug level. */
|
||||
|
|
@ -220,6 +223,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 *);
|
||||
|
||||
// misc.
|
||||
void ec_master_attach_slave_configs(ec_master_t *);
|
||||
|
|
|
|||
|
|
@ -332,6 +332,11 @@ void ec_print_data(const uint8_t *data, /**< pointer to data */
|
|||
printk("\n");
|
||||
EC_DBG("");
|
||||
}
|
||||
if (i == 128 && size > 256)
|
||||
{
|
||||
EC_DBG("dropped %d bytes\n",size-128-i);
|
||||
i = size - 128;
|
||||
}
|
||||
}
|
||||
printk("\n");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ void ec_slave_init(
|
|||
)
|
||||
{
|
||||
unsigned int i;
|
||||
int ret;
|
||||
|
||||
slave->master = master;
|
||||
slave->ring_position = ring_position;
|
||||
|
|
@ -147,6 +148,20 @@ void ec_slave_init(
|
|||
|
||||
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 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
|
|||
|
||||
if (slave->sii_words)
|
||||
kfree(slave->sii_words);
|
||||
ec_fsm_slave_clear(&slave->fsm);
|
||||
ec_datagram_clear(&slave->fsm_datagram);
|
||||
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@
|
|||
#include "pdo.h"
|
||||
#include "sync.h"
|
||||
#include "sdo.h"
|
||||
#include "fsm_slave.h"
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
|
|
@ -157,6 +158,9 @@ struct ec_slave
|
|||
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. */
|
||||
};
|
||||
|
||||
/*****************************************************************************/
|
||||
|
|
|
|||
Loading…
Reference in New Issue