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

Merged changes by Martin Troxler.

This commit is contained in:
Florian Pose 2009-12-14 13:25:50 +01:00
parent 699b184f6f cb33213076
commit 344ec90545
17 changed files with 512 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/ecrt.h
View File

@ -698,6 +698,15 @@ void ecrt_master_deactivate(
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,

15
lib/master.c
View File

@ -344,6 +344,21 @@ void ecrt_master_deactivate(ec_master_t *master)
}
}
/*****************************************************************************/
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)

1
master/Kbuild.in
View File

@ -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 \

1
master/Makefile.am
View File

@ -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 \

44
master/cdev.c
View File

@ -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);
@ -816,7 +818,7 @@ int ec_cdev_ioctl_slave_sdo_upload(
// 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 @@ 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);
@ -917,7 +924,7 @@ int ec_cdev_ioctl_slave_sdo_download(
// 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 @@ 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;
@ -1663,6 +1673,32 @@ int ec_cdev_ioctl_deactivate(
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 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
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;
}

53
master/fsm_master.c
View File

@ -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

2
master/fsm_master.h
View File

@ -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 *);
/*****************************************************************************/

4
master/fsm_sii.c
View File

@ -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;
}

185
master/fsm_slave.c Normal file
View File

@ -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;
}

70
master/fsm_slave.h Normal file
View File

@ -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__

6
master/globals.h
View File

@ -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

1
master/ioctl.h
View File

@ -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)
/*****************************************************************************/

163
master/master.c
View File

@ -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 @@ void ec_master_init_static(void)
{
#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 @@ 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);
master->max_queue_size = EC_MAX_DATA_SIZE;
INIT_LIST_HEAD(&master->domains);
master->debug_level = debug_level;
@ -684,6 +690,91 @@ 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 *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 @@ 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 +1094,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 +1107,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 +1144,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 +1899,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

5
master/master.h
View File

@ -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. */
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_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 *);

8
master/module.c
View File

@ -318,6 +318,8 @@ static int ec_mac_parse(uint8_t *mac, const char *src, int allow_empty)
/*****************************************************************************/
/** 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 @@ void ec_print_data(const uint8_t *data, /**< pointer to data */
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");
}

18
master/slave.c
View File

@ -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);
}
/*****************************************************************************/

4
master/slave.h
View File

@ -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. */
};
/*****************************************************************************/