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

Implemented reading Sercos-over-EtherCAT services. To be continued...

This commit is contained in:
Florian Pose 2010-03-01 18:33:42 +01:00
parent d11fd92168
commit 3850763eca
23 changed files with 1454 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
master/Kbuild.in
View File

@ -44,12 +44,13 @@ 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 \
fsm_slave.o \
fsm_slave_config.o \
fsm_slave_scan.o \
fsm_soe.o \
mailbox.o \
master.o \
module.o \
@ -61,6 +62,7 @@ ec_master-objs := \
sdo_request.o \
slave.o \
slave_config.o \
soe_request.o \
sync.o \
sync_config.o \
voe_handler.o

4
master/Makefile.am
View File

@ -43,12 +43,13 @@ 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 \
fsm_slave.c fsm_slave.h \
fsm_slave_config.c fsm_slave_config.h \
fsm_slave_scan.c fsm_slave_scan.h \
fsm_soe.c fsm_soe.h \
globals.h \
ioctl.h \
mailbox.c mailbox.h \
@ -62,6 +63,7 @@ noinst_HEADERS = \
sdo_request.c sdo_request.h \
slave.c slave.h \
slave_config.c slave_config.h \
soe_request.c soe_request.h \
sync.c sync.h \
sync_config.c sync_config.h \
voe_handler.c voe_handler.h

235
master/cdev.c
View File

@ -284,10 +284,12 @@ int ec_cdev_ioctl_slave(
data.ports[i].desc = slave->ports[i].desc;
data.ports[i].link.link_up = slave->ports[i].link.link_up;
data.ports[i].link.loop_closed = slave->ports[i].link.loop_closed;
data.ports[i].link.signal_detected = slave->ports[i].link.signal_detected;
data.ports[i].link.signal_detected =
slave->ports[i].link.signal_detected;
data.ports[i].receive_time = slave->ports[i].receive_time;
if (slave->ports[i].next_slave) {
data.ports[i].next_slave = slave->ports[i].next_slave->ring_position;
data.ports[i].next_slave =
slave->ports[i].next_slave->ring_position;
} else {
data.ports[i].next_slave = 0xffff;
}
@ -825,7 +827,8 @@ int ec_cdev_ioctl_slave_sdo_upload(
}
if (master->debug_level)
EC_DBG("Schedule SDO upload request for slave %u\n",request.slave->ring_position);
EC_DBG("Schedule SDO upload request for slave %u\n",
request.slave->ring_position);
// schedule request.
list_add_tail(&request.list, &request.slave->slave_sdo_requests);
@ -847,10 +850,12 @@ int ec_cdev_ioctl_slave_sdo_upload(
}
// wait until master FSM has finished processing
wait_event(request.slave->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);
EC_DBG("Scheduled SDO upload request for slave %u done\n",
request.slave->ring_position);
data.abort_code = request.req.abort_code;
@ -931,7 +936,8 @@ int ec_cdev_ioctl_slave_sdo_download(
}
if (master->debug_level)
EC_DBG("Schedule SDO download request for slave %u\n",request.slave->ring_position);
EC_DBG("Schedule SDO download request for slave %u\n",
request.slave->ring_position);
// schedule request.
list_add_tail(&request.list, &request.slave->slave_sdo_requests);
@ -953,10 +959,12 @@ int ec_cdev_ioctl_slave_sdo_download(
}
// wait until master FSM has finished processing
wait_event(request.slave->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);
EC_DBG("Scheduled SDO download request for slave %u done\n",
request.slave->ring_position);
data.abort_code = request.req.abort_code;
@ -1120,7 +1128,8 @@ int ec_cdev_ioctl_slave_reg_read(
return 0;
if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
EC_ERR("Failed to allocate %u bytes for register data.\n", data.length);
EC_ERR("Failed to allocate %u bytes for register data.\n",
data.length);
return -ENOMEM;
}
@ -1197,7 +1206,8 @@ int ec_cdev_ioctl_slave_reg_write(
return 0;
if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
EC_ERR("Failed to allocate %u bytes for register data.\n", data.length);
EC_ERR("Failed to allocate %u bytes for register data.\n",
data.length);
return -ENOMEM;
}
@ -2240,7 +2250,8 @@ int ec_cdev_ioctl_sc_sdo(
up(&master->master_sem); // FIXME
if (data.complete_access) {
ret = ecrt_slave_config_complete_sdo(sc, data.index, sdo_data, data.size);
ret = ecrt_slave_config_complete_sdo(sc,
data.index, sdo_data, data.size);
} else {
ret = ecrt_slave_config_sdo(sc, data.index, data.subindex, sdo_data,
data.size);
@ -3102,7 +3113,8 @@ int ec_cdev_ioctl_slave_foe_read(
}
// wait until master FSM has finished processing
wait_event(request.slave->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;
@ -3212,7 +3224,8 @@ int ec_cdev_ioctl_slave_foe_write(
}
// wait until master FSM has finished processing
wait_event(request.slave->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;
@ -3226,7 +3239,195 @@ int ec_cdev_ioctl_slave_foe_write(
ec_foe_request_clear(&request.req);
if (master->debug_level) {
printk("Finished FoE writing.\n");
EC_DBG("Finished FoE writing.\n");
}
return retval;
}
/*****************************************************************************/
/** Read an SoE IDN.
*/
int ec_cdev_ioctl_slave_soe_read(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg /**< ioctl() argument. */
)
{
ec_ioctl_slave_soe_t data;
ec_master_soe_request_t request;
int retval;
if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
return -EFAULT;
}
ec_soe_request_init(&request.req);
ec_soe_request_set_idn(&request.req, data.idn);
ec_soe_request_read(&request.req);
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(request.slave = ec_master_find_slave(
master, 0, data.slave_position))) {
up(&master->master_sem);
ec_soe_request_clear(&request.req);
EC_ERR("Slave %u does not exist!\n", data.slave_position);
return -EINVAL;
}
// schedule request.
list_add_tail(&request.list, &request.slave->soe_requests);
up(&master->master_sem);
if (master->debug_level) {
EC_DBG("Scheduled SoE read request on slave %u.\n",
request.slave->ring_position);
}
// wait for processing through FSM
if (wait_event_interruptible(request.slave->soe_queue,
request.req.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
if (request.req.state == EC_INT_REQUEST_QUEUED) {
list_del(&request.list);
up(&master->master_sem);
ec_soe_request_clear(&request.req);
return -EINTR;
}
// request already processing: interrupt not possible.
up(&master->master_sem);
}
// wait until master FSM has finished processing
wait_event(request.slave->soe_queue,
request.req.state != EC_INT_REQUEST_BUSY);
data.error_code = request.req.error_code;
if (master->debug_level) {
EC_DBG("Read %zd bytes via SoE.\n", request.req.data_size);
}
if (request.req.state != EC_INT_REQUEST_SUCCESS) {
data.data_size = 0;
retval = -EIO;
} else {
if (request.req.data_size > data.mem_size) {
EC_ERR("Buffer too small.\n");
ec_soe_request_clear(&request.req);
return -EOVERFLOW;
}
data.data_size = request.req.data_size;
if (copy_to_user((void __user *) data.data,
request.req.data, data.data_size)) {
ec_soe_request_clear(&request.req);
return -EFAULT;
}
retval = 0;
}
if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
retval = -EFAULT;
}
if (master->debug_level)
EC_DBG("SoE read request finished on slave %u.\n",
request.slave->ring_position);
ec_soe_request_clear(&request.req);
return retval;
}
/*****************************************************************************/
/** Write an IDN to a slave via SoE.
*/
int ec_cdev_ioctl_slave_soe_write(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg /**< ioctl() argument. */
)
{
ec_ioctl_slave_soe_t data;
ec_master_soe_request_t request;
int retval;
if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
return -EFAULT;
}
INIT_LIST_HEAD(&request.list);
ec_soe_request_init(&request.req);
ec_soe_request_set_idn(&request.req, data.idn);
if (ec_soe_request_alloc(&request.req, data.mem_size)) {
ec_soe_request_clear(&request.req);
return -ENOMEM;
}
if (copy_from_user(request.req.data,
(void __user *) data.data, data.mem_size)) {
ec_soe_request_clear(&request.req);
return -EFAULT;
}
request.req.data_size = data.mem_size;
ec_soe_request_write(&request.req);
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(request.slave = ec_master_find_slave(
master, 0, data.slave_position))) {
up(&master->master_sem);
EC_ERR("Slave %u does not exist!\n", data.slave_position);
ec_soe_request_clear(&request.req);
return -EINVAL;
}
if (master->debug_level) {
EC_DBG("Scheduling SoE write request.\n");
}
// schedule SoE write request.
list_add_tail(&request.list, &request.slave->soe_requests);
up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(request.slave->soe_queue,
request.req.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
if (request.req.state == EC_INT_REQUEST_QUEUED) {
// abort request
list_del(&request.list);
up(&master->master_sem);
ec_soe_request_clear(&request.req);
return -EINTR;
}
up(&master->master_sem);
}
// wait until master FSM has finished processing
wait_event(request.slave->soe_queue,
request.req.state != EC_INT_REQUEST_BUSY);
//data.result = request.req.result;
retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
retval = -EFAULT;
}
ec_soe_request_clear(&request.req);
if (master->debug_level) {
EC_DBG("Finished SoE writing.\n");
}
return retval;
@ -3354,6 +3555,12 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_slave_foe_write(master, arg);
case EC_IOCTL_SLAVE_SOE_READ:
return ec_cdev_ioctl_slave_soe_read(master, arg);
case EC_IOCTL_SLAVE_SOE_WRITE:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_slave_soe_write(master, arg);
case EC_IOCTL_CONFIG:
return ec_cdev_ioctl_config(master, arg);
case EC_IOCTL_CONFIG_PDO:

2
master/fsm_coe.c
View File

@ -1964,7 +1964,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
if (rec_size < 6 + fsm->complete_size) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Received currupted SDO expedited upload"
EC_ERR("Received corrupted SDO expedited upload"
" response (only %zu bytes)!\n", rec_size);
ec_print_data(data, rec_size);
return;

6
master/fsm_master.c
View File

@ -426,8 +426,8 @@ 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("Internal SDO request for slave %u timed out...\n",
slave->ring_position);
EC_DBG("Internal SDO request for slave %u"
" timed out...\n", slave->ring_position);
continue;
}
@ -472,7 +472,7 @@ void ec_fsm_master_action_idle(
if (ec_fsm_master_action_process_sdo(fsm))
return;
// enable processing of SDO/FOE requests
// enable processing of requests
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {

13
master/fsm_master.h
View File

@ -41,6 +41,7 @@
#include "datagram.h"
#include "foe_request.h"
#include "sdo_request.h"
#include "soe_request.h"
#include "fsm_slave_config.h"
#include "fsm_slave_scan.h"
#include "fsm_pdo.h"
@ -85,7 +86,7 @@ typedef struct {
/*****************************************************************************/
/** FoE write request.
/** FoE request.
*/
typedef struct {
struct list_head list; /**< List head. */
@ -95,6 +96,16 @@ typedef struct {
/*****************************************************************************/
/** SoE request.
*/
typedef struct {
struct list_head list; /**< List head. */
ec_slave_t *slave; /**< EtherCAT slave. */
ec_soe_request_t req; /**< SoE request. */
} ec_master_soe_request_t;
/*****************************************************************************/
typedef struct ec_fsm_master ec_fsm_master_t; /**< \see ec_fsm_master */
/** Finite state machine of an EtherCAT master.

196
master/fsm_slave.c
View File

@ -44,10 +44,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 *);
int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
/*****************************************************************************/
@ -71,6 +72,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);
ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
}
/*****************************************************************************/
@ -84,6 +86,7 @@ void ec_fsm_slave_clear(
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_fsm_foe_clear(&fsm->fsm_foe);
ec_fsm_soe_clear(&fsm->fsm_soe);
}
/*****************************************************************************/
@ -107,11 +110,9 @@ void ec_fsm_slave_exec(
return;
}
/*****************************************************************************/
/** Sets the current state of the state machine to READY
*
*/
void ec_fsm_slave_ready(
ec_fsm_slave_t *fsm /**< Slave state machine. */
@ -119,23 +120,18 @@ void ec_fsm_slave_ready(
{
if (fsm->state == ec_fsm_slave_state_idle) {
if (fsm->slave->master->debug_level) {
EC_DBG("Slave %u ready for SDO/FOE.\n",
EC_DBG("Slave %u ready for requests.\n",
fsm->slave->ring_position);
}
fsm->state = ec_fsm_slave_state_ready;
}
return;
}
/******************************************************************************
* Slave state machine
*****************************************************************************/
/*****************************************************************************/
/** Slave state: IDLE.
*
*
*/
void ec_fsm_slave_state_idle(
ec_fsm_slave_t *fsm /**< Slave state machine. */
@ -148,8 +144,6 @@ void ec_fsm_slave_state_idle(
/*****************************************************************************/
/** Slave state: READY.
*
*
*/
void ec_fsm_slave_state_ready(
ec_fsm_slave_t *fsm /**< Slave state machine. */
@ -159,9 +153,13 @@ void ec_fsm_slave_state_ready(
if (ec_fsm_slave_action_process_sdo(fsm))
return;
// Check for pending FOE requests
// Check for pending FoE requests
if (ec_fsm_slave_action_process_foe(fsm))
return;
// Check for pending SoE requests
if (ec_fsm_slave_action_process_soe(fsm))
return;
}
/*****************************************************************************/
@ -220,6 +218,44 @@ int ec_fsm_slave_action_process_sdo(
return 0;
}
/*****************************************************************************/
/** 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_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(&slave->sdo_queue);
fsm->sdo_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->state = ec_fsm_slave_state_ready;
}
/*****************************************************************************/
@ -263,47 +299,6 @@ int ec_fsm_slave_action_process_foe(
return 0;
}
/*****************************************************************************/
/** 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_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(&slave->sdo_queue);
fsm->sdo_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->state = ec_fsm_slave_state_ready;
}
/*****************************************************************************/
/** Slave state: FOE REQUEST.
@ -345,3 +340,96 @@ void ec_fsm_slave_state_foe_request(
}
/*****************************************************************************/
/** Check for pending SoE requests and process one.
*
* \return non-zero, if a request is processed.
*/
int ec_fsm_slave_action_process_soe(
ec_fsm_slave_t *fsm /**< Slave state machine. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_master_soe_request_t *req, *next;
// search the first request to be processed
list_for_each_entry_safe(req, next, &slave->soe_requests, list) {
list_del_init(&req->list); // dequeue
if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
EC_WARN("Aborting SoE request, slave %u has ERROR.\n",
slave->ring_position);
req->req.state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->soe_queue);
fsm->state = ec_fsm_slave_state_idle;
return 0;
}
if (slave->current_state == EC_SLAVE_STATE_INIT) {
EC_WARN("Aborting SoE request, slave %u is in INIT.\n",
slave->ring_position);
req->req.state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->soe_queue);
fsm->state = ec_fsm_slave_state_idle;
return 0;
}
req->req.state = EC_INT_REQUEST_BUSY;
// Found pending request. Execute it!
if (master->debug_level)
EC_DBG("Processing SoE request for slave %u...\n",
slave->ring_position);
// Start SoE transfer
fsm->soe_request = &req->req;
fsm->state = ec_fsm_slave_state_soe_request;
ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req);
ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return 1;
}
return 0;
}
/*****************************************************************************/
/** Slave state: SOE_REQUEST.
*/
void ec_fsm_slave_state_soe_request(
ec_fsm_slave_t *fsm /**< Slave state machine. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *request = fsm->soe_request;
if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
return;
}
if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
EC_DBG("Failed to process SoE request for slave %u.\n",
fsm->slave->ring_position);
request->state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->soe_queue);
fsm->soe_request = NULL;
fsm->state = ec_fsm_slave_state_idle;
return;
}
if (master->debug_level)
EC_DBG("Finished SoE request for slave %u.\n",
fsm->slave->ring_position);
// SoE request finished
request->state = EC_INT_REQUEST_SUCCESS;
wake_up(&slave->soe_queue);
fsm->soe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}
/*****************************************************************************/

3
master/fsm_slave.h
View File

@ -41,6 +41,7 @@
#include "sdo_request.h"
#include "fsm_coe.h"
#include "fsm_foe.h"
#include "fsm_soe.h"
typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */
@ -54,9 +55,11 @@ struct ec_fsm_slave {
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_soe_request_t *soe_request; /**< SoE request to process. */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
ec_fsm_foe_t fsm_foe; /**< FoE state machine */
ec_fsm_soe_t fsm_soe; /**< SoE state machine */
};
/*****************************************************************************/

389
master/fsm_soe.c Normal file
View File

@ -0,0 +1,389 @@
/******************************************************************************
*
* $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 SoE state machines.
*/
/*****************************************************************************/
#include "globals.h"
#include "master.h"
#include "mailbox.h"
#include "fsm_soe.h"
/*****************************************************************************/
/** Mailbox type for SoE.
*/
#define EC_MBOX_TYPE_SOE 0x05
#define EC_SOE_OPCODE_READ_REQUEST 0x01
#define EC_SOE_OPCODE_READ_RESPONSE 0x02
#define EC_SOE_READ_REQUEST_SIZE 0x04
#define EC_SOE_READ_RESPONSE_SIZE 0x04
#define EC_SOE_RESPONSE_TIMEOUT 1000
/*****************************************************************************/
void ec_fsm_soe_read_start(ec_fsm_soe_t *);
void ec_fsm_soe_read_request(ec_fsm_soe_t *);
void ec_fsm_soe_read_check(ec_fsm_soe_t *);
void ec_fsm_soe_read_response(ec_fsm_soe_t *);
void ec_fsm_soe_end(ec_fsm_soe_t *);
void ec_fsm_soe_error(ec_fsm_soe_t *);
/*****************************************************************************/
/** Constructor.
*/
void ec_fsm_soe_init(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< datagram */
)
{
fsm->state = NULL;
fsm->datagram = datagram;
}
/*****************************************************************************/
/** Destructor.
*/
void ec_fsm_soe_clear(
ec_fsm_soe_t *fsm /**< finite state machine */
)
{
}
/*****************************************************************************/
/** Starts to transfer an IDN to/from a slave.
*/
void ec_fsm_soe_transfer(
ec_fsm_soe_t *fsm, /**< State machine. */
ec_slave_t *slave, /**< EtherCAT slave. */
ec_soe_request_t *request /**< SoE request. */
)
{
fsm->slave = slave;
fsm->request = request;
if (request->dir == EC_DIR_OUTPUT) {
//fsm->state = ec_fsm_soe_write_start;
} else {
fsm->state = ec_fsm_soe_read_start;
}
}
/*****************************************************************************/
/**
Executes the current state of the state machine.
\return false, if state machine has terminated
*/
int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)
{
fsm->state(fsm);
return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
}
/*****************************************************************************/
/**
Returns, if the state machine terminated with success.
\return non-zero if successful.
*/
int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)
{
return fsm->state == ec_fsm_soe_end;
}
/******************************************************************************
* SoE read state machine
*****************************************************************************/
/** SoE state: READ START.
*/
void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *request = fsm->request;
uint8_t *data;
if (master->debug_level)
EC_DBG("Reading IDN 0x%04X from slave %u.\n",
request->idn, slave->ring_position);
if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);
fsm->state = ec_fsm_soe_error;
return;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
EC_SOE_READ_REQUEST_SIZE);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
return;
}
EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST);
EC_WRITE_U8(data + 1, 1 << 6); // request value
EC_WRITE_U16(data + 2, request->idn);
if (master->debug_level) {
EC_DBG("SCC read request:\n");
ec_print_data(data, EC_SOE_READ_REQUEST_SIZE);
}
fsm->request->jiffies_sent = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_request;
}
/*****************************************************************************/
/** SoE state: READ REQUEST.
*/
void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Failed to receive SoE read request for slave %u: ",
slave->ring_position);
ec_datagram_print_state(datagram);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
if (datagram->working_counter != 1) {
if (!datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
return;
}
}
fsm->state = ec_fsm_soe_error;
EC_ERR("Reception of SoE read request for IDN 0x%04x failed"
" after %u ms on slave %u: ",
fsm->request->idn, (u32) diff_ms,
fsm->slave->ring_position);
ec_datagram_print_wc_error(datagram);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
}
/*****************************************************************************/
/** CoE state: READ CHECK.
*/
void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ",
slave->ring_position);
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ",
slave->ring_position);
ec_datagram_print_wc_error(datagram);
return;
}
if (!ec_slave_mbox_check(datagram)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x"
" read response on slave %u.\n", (u32) diff_ms,
fsm->request->idn, slave->ring_position);
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_response;
}
/*****************************************************************************/
/** SoE state: READ RESPONSE.
*/
void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, opcode, error_flag, value_included;
size_t rec_size, data_size;
ec_soe_request_t *req = fsm->request;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Failed to receive SoE read response datagram for"
" slave %u: ", slave->ring_position);
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Reception of SoE read response failed on slave %u: ",
slave->ring_position);
ec_datagram_print_wc_error(datagram);
return;
}
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
return;
}
if (master->debug_level) {
EC_DBG("SCC read response:\n");
ec_print_data(data, rec_size);
}
if (mbox_prot != EC_MBOX_TYPE_SOE) {
fsm->state = ec_fsm_soe_error;
EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
return;
}
if (rec_size < EC_SOE_READ_RESPONSE_SIZE) {
fsm->state = ec_fsm_soe_error;
EC_ERR("Received currupted SoE read response (%zu bytes)!\n",
rec_size);
ec_print_data(data, rec_size);
return;
}
opcode = EC_READ_U8(data) & 0x3;
if (opcode != EC_SOE_OPCODE_READ_RESPONSE) {
EC_ERR("Received no read response (opcode %x).\n", opcode);
ec_print_data(data, rec_size);
fsm->state = ec_fsm_soe_error;
return;
}
error_flag = (EC_READ_U8(data) >> 4) & 1;
if (error_flag) {
req->error_code = EC_READ_U16(data + rec_size - 2);
EC_ERR("Received error response: 0x%04x.\n",
req->error_code);
fsm->state = ec_fsm_soe_error;
return;
} else {
req->error_code = 0x0000;
}
value_included = (EC_READ_U8(data + 1) >> 6) & 1;
if (!value_included) {
EC_ERR("No value included!\n");
fsm->state = ec_fsm_soe_error;
return;
}
data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE;
if (ec_soe_request_copy_data(req,
data + EC_SOE_READ_RESPONSE_SIZE, data_size)) {
fsm->state = ec_fsm_soe_error;
return;
}
if (master->debug_level) {
EC_DBG("IDN data:\n");
ec_print_data(req->data, req->data_size);
}
fsm->state = ec_fsm_soe_end; // success
}
/*****************************************************************************/
/** State: ERROR.
*/
void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/
/** State: END.
*/
void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/

73
master/fsm_soe.h Normal file
View File

@ -0,0 +1,73 @@
/******************************************************************************
*
* $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 CoE state machines.
*/
/*****************************************************************************/
#ifndef __EC_FSM_SOE_H__
#define __EC_FSM_SOE_H__
#include "globals.h"
#include "datagram.h"
#include "slave.h"
#include "soe_request.h"
/*****************************************************************************/
typedef struct ec_fsm_soe ec_fsm_soe_t; /**< \see ec_fsm_soe */
/** Finite state machines for the Sercos over EtherCAT protocol.
*/
struct ec_fsm_soe {
ec_slave_t *slave; /**< slave the FSM runs on */
ec_datagram_t *datagram; /**< datagram used in the state machine */
unsigned int retries; /**< retries upon datagram timeout */
void (*state)(ec_fsm_soe_t *); /**< CoE state function */
unsigned long jiffies_start; /**< CoE timestamp. */
ec_soe_request_t *request; /**< SoE request */
};
/*****************************************************************************/
void ec_fsm_soe_init(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_clear(ec_fsm_soe_t *);
void ec_fsm_soe_transfer(ec_fsm_soe_t *, ec_slave_t *, ec_soe_request_t *);
int ec_fsm_soe_exec(ec_fsm_soe_t *);
int ec_fsm_soe_success(ec_fsm_soe_t *);
/*****************************************************************************/
#endif

113
master/ioctl.h
View File

@ -56,7 +56,7 @@
*
* Increment this when changing the ioctl interface!
*/
#define EC_IOCTL_VERSION_MAGIC 1
#define EC_IOCTL_VERSION_MAGIC 2
// Command-line tool
#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t)
@ -80,57 +80,59 @@
#define EC_IOCTL_SLAVE_REG_WRITE EC_IOW(0x12, ec_ioctl_slave_reg_t)
#define EC_IOCTL_SLAVE_FOE_READ EC_IOWR(0x13, ec_ioctl_slave_foe_t)
#define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x14, ec_ioctl_slave_foe_t)
#define EC_IOCTL_CONFIG EC_IOWR(0x15, ec_ioctl_config_t)
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x16, ec_ioctl_config_pdo_t)
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x17, ec_ioctl_config_pdo_entry_t)
#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x18, ec_ioctl_config_sdo_t)
#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x15, ec_ioctl_slave_soe_t)
#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x16, ec_ioctl_slave_soe_t)
#define EC_IOCTL_CONFIG EC_IOWR(0x17, ec_ioctl_config_t)
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x18, ec_ioctl_config_pdo_t)
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x19, ec_ioctl_config_pdo_entry_t)
#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x1a, ec_ioctl_config_sdo_t)
#ifdef EC_EOE
#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x19, ec_ioctl_eoe_handler_t)
#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x1b, ec_ioctl_eoe_handler_t)
#endif
// Application interface
#define EC_IOCTL_REQUEST EC_IO(0x1a)
#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x1b)
#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x1c, ec_ioctl_config_t)
#define EC_IOCTL_ACTIVATE EC_IOR(0x1d, size_t)
#define EC_IOCTL_DEACTIVATE EC_IO(0x1e)
#define EC_IOCTL_SEND EC_IO(0x1f)
#define EC_IOCTL_RECEIVE EC_IO(0x20)
#define EC_IOCTL_MASTER_STATE EC_IOR(0x21, ec_master_state_t)
#define EC_IOCTL_APP_TIME EC_IOW(0x22, ec_ioctl_app_time_t)
#define EC_IOCTL_SYNC_REF EC_IO(0x23)
#define EC_IOCTL_SYNC_SLAVES EC_IO(0x24)
#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x25)
#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x26, uint32_t)
#define EC_IOCTL_SC_SYNC EC_IOW(0x27, ec_ioctl_config_t)
#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x28, ec_ioctl_config_t)
#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x29, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x2a, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x2b, ec_ioctl_add_pdo_entry_t)
#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x2c, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x2d, ec_ioctl_reg_pdo_entry_t)
#define EC_IOCTL_SC_DC EC_IOW(0x2e, ec_ioctl_config_t)
#define EC_IOCTL_SC_SDO EC_IOW(0x2f, ec_ioctl_sc_sdo_t)
#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x20, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SC_VOE EC_IOWR(0x31, ec_ioctl_voe_t)
#define EC_IOCTL_SC_STATE EC_IOWR(0x32, ec_ioctl_sc_state_t)
#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x33)
#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x34)
#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x35)
#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x36, ec_ioctl_domain_state_t)
#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x37, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x38, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x39, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x3a, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x3b, ec_ioctl_sdo_request_t)
#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x3c, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x3d, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_READ EC_IOW(0x3e, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x3f, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_WRITE EC_IOWR(0x40, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_EXEC EC_IOWR(0x41, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_DATA EC_IOWR(0x42, ec_ioctl_voe_t)
#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x43, size_t)
#define EC_IOCTL_REQUEST EC_IO(0x1c)
#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x1d)
#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x1e, ec_ioctl_config_t)
#define EC_IOCTL_ACTIVATE EC_IOR(0x1f, size_t)
#define EC_IOCTL_DEACTIVATE EC_IO(0x20)
#define EC_IOCTL_SEND EC_IO(0x21)
#define EC_IOCTL_RECEIVE EC_IO(0x22)
#define EC_IOCTL_MASTER_STATE EC_IOR(0x23, ec_master_state_t)
#define EC_IOCTL_APP_TIME EC_IOW(0x24, ec_ioctl_app_time_t)
#define EC_IOCTL_SYNC_REF EC_IO(0x25)
#define EC_IOCTL_SYNC_SLAVES EC_IO(0x26)
#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x27)
#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x28, uint32_t)
#define EC_IOCTL_SC_SYNC EC_IOW(0x29, ec_ioctl_config_t)
#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x2a, ec_ioctl_config_t)
#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x2b, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x2c, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x2d, ec_ioctl_add_pdo_entry_t)
#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x2e, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x2f, ec_ioctl_reg_pdo_entry_t)
#define EC_IOCTL_SC_DC EC_IOW(0x20, ec_ioctl_config_t)
#define EC_IOCTL_SC_SDO EC_IOW(0x31, ec_ioctl_sc_sdo_t)
#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x32, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SC_VOE EC_IOWR(0x33, ec_ioctl_voe_t)
#define EC_IOCTL_SC_STATE EC_IOWR(0x34, ec_ioctl_sc_state_t)
#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x35)
#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x36)
#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x37)
#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x38, ec_ioctl_domain_state_t)
#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x39, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x3a, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x3b, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x3c, ec_ioctl_sdo_request_t)
#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x3d, ec_ioctl_sdo_request_t)
#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x3e, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x3f, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_READ EC_IOW(0x40, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x41, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_WRITE EC_IOWR(0x42, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_EXEC EC_IOWR(0x43, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_DATA EC_IOWR(0x44, ec_ioctl_voe_t)
#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x45, size_t)
/*****************************************************************************/
@ -402,6 +404,21 @@ typedef struct {
/*****************************************************************************/
typedef struct {
// inputs
uint16_t slave_position;
uint16_t idn;
uint32_t mem_size;
uint8_t *data;
// outputs
uint32_t data_size;
uint32_t result;
uint16_t error_code;
} ec_ioctl_slave_soe_t;
/*****************************************************************************/
typedef struct {
// inputs
uint32_t config_index;

21
master/master.c
View File

@ -417,7 +417,8 @@ void ec_master_clear_slaves(ec_master_t *master)
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",
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);
@ -431,11 +432,27 @@ void ec_master_clear_slaves(ec_master_t *master)
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",
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);
}
// SoE requests
while (1) {
ec_master_soe_request_t *request;
if (list_empty(&slave->soe_requests))
break;
// get first request
request = list_entry(slave->soe_requests.next,
ec_master_soe_request_t, list);
list_del_init(&request->list); // dequeue
EC_INFO("Discarding SoE request,"
" slave %u does not exist anymore.\n",
slave->ring_position);
request->req.state = EC_INT_REQUEST_FAILURE;
wake_up(&slave->soe_queue);
}
ec_slave_clear(slave);
}

2
master/sdo_request.c
View File

@ -191,7 +191,7 @@ int ec_sdo_request_timed_out(const ec_sdo_request_t *req /**< SDO request. */)
}
/*****************************************************************************
* Realtime interface.
* Application interface.
****************************************************************************/
void ecrt_sdo_request_timeout(ec_sdo_request_t *req, uint32_t timeout)

3
master/slave.c
View File

@ -155,6 +155,9 @@ void ec_slave_init(
INIT_LIST_HEAD(&slave->foe_requests);
init_waitqueue_head(&slave->foe_queue);
INIT_LIST_HEAD(&slave->soe_requests);
init_waitqueue_head(&slave->soe_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);

7
master/slave.h
View File

@ -163,8 +163,11 @@ struct ec_slave
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. */
wait_queue_head_t foe_queue; /**< Wait queue for FoE requests from user
space. */
struct list_head soe_requests; /**< FoE write requests. */
wait_queue_head_t soe_queue; /**< Wait queue for SoE requests from user
space. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
};

198
master/soe_request.c Normal file
View File

@ -0,0 +1,198 @@
/******************************************************************************
*
* $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
* Sercos-over-EtherCAT request functions.
*/
/*****************************************************************************/
#include <linux/module.h>
#include <linux/jiffies.h>
#include "soe_request.h"
/*****************************************************************************/
/** Default timeout in ms to wait for SoE responses.
*/
#define EC_SOE_REQUEST_RESPONSE_TIMEOUT 1000
/*****************************************************************************/
void ec_soe_request_clear_data(ec_soe_request_t *);
/*****************************************************************************/
/** SoE request constructor.
*/
void ec_soe_request_init(
ec_soe_request_t *req /**< SoE request. */
)
{
req->data = NULL;
req->mem_size = 0;
req->data_size = 0;
req->dir = EC_DIR_INVALID;
req->state = EC_INT_REQUEST_INIT;
//req->jiffies_start = 0U;
req->jiffies_sent = 0U;
req->error_code = 0x0000;
}
/*****************************************************************************/
/** SoE request destructor.
*/
void ec_soe_request_clear(
ec_soe_request_t *req /**< SoE request. */
)
{
ec_soe_request_clear_data(req);
}
/*****************************************************************************/
/** Set IDN.
*/
void ec_soe_request_set_idn(
ec_soe_request_t *req, /**< SoE request. */
uint16_t idn /** IDN. */
)
{
req->idn = idn;
}
#if 0
/*****************************************************************************/
/** Copy another SoE request.
*
* \attention Only the index subindex and data are copied.
*/
int ec_soe_request_copy(
ec_soe_request_t *req, /**< SoE request. */
const ec_soe_request_t *other /**< Other SoE request to copy from. */
)
{
req->complete_access = other->complete_access;
req->index = other->index;
req->subindex = other->subindex;
return ec_soe_request_copy_data(req, other->data, other->data_size);
}
#endif
/*****************************************************************************/
/** Free allocated memory.
*/
void ec_soe_request_clear_data(
ec_soe_request_t *req /**< SoE request. */
)
{
if (req->data) {
kfree(req->data);
req->data = NULL;
}
req->mem_size = 0;
req->data_size = 0;
}
/*****************************************************************************/
/** Pre-allocates the data memory.
*
* If the \a mem_size is already bigger than \a size, nothing is done.
*
* \return 0 on success, otherwise -ENOMEM.
*/
int ec_soe_request_alloc(
ec_soe_request_t *req, /**< SoE request. */
size_t size /**< Data size to allocate. */
)
{
if (size <= req->mem_size)
return 0;
ec_soe_request_clear_data(req);
if (!(req->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
EC_ERR("Failed to allocate %zu bytes of SoE memory.\n", size);
return -ENOMEM;
}
req->mem_size = size;
req->data_size = 0;
return 0;
}
/*****************************************************************************/
/** Copies SoE data from an external source.
*
* If the \a mem_size is to small, new memory is allocated.
*
* \retval 0 Success.
* \retval <0 Error code.
*/
int ec_soe_request_copy_data(
ec_soe_request_t *req, /**< SoE request. */
const uint8_t *source, /**< Source data. */
size_t size /**< Number of bytes in \a source. */
)
{
int ret = ec_soe_request_alloc(req, size);
if (ret < 0)
return ret;
memcpy(req->data, source, size);
req->data_size = size;
return 0;
}
/*****************************************************************************/
void ec_soe_request_read(ec_soe_request_t *req)
{
req->dir = EC_DIR_INPUT;
req->state = EC_INT_REQUEST_QUEUED;
req->error_code = 0x0000;
}
/*****************************************************************************/
void ec_soe_request_write(ec_soe_request_t *req)
{
req->dir = EC_DIR_OUTPUT;
req->state = EC_INT_REQUEST_QUEUED;
req->error_code = 0x0000;
}
/*****************************************************************************/

80
master/soe_request.h Normal file
View File

@ -0,0 +1,80 @@
/******************************************************************************
*
* $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 SoE request structure.
*/
/*****************************************************************************/
#ifndef __EC_SOE_REQUEST_H__
#define __EC_SOE_REQUEST_H__
#include <linux/list.h>
#include "globals.h"
/*****************************************************************************/
/** Sercos-over-EtherCAT request.
*/
typedef struct {
struct list_head list; /**< List item. */
uint16_t idn; /**< Sercos ID-Number. */
uint8_t *data; /**< Pointer to SDO data. */
size_t mem_size; /**< Size of SDO data memory. */
size_t data_size; /**< Size of SDO data. */
ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means writing to the
slave, EC_DIR_INPUT means reading from the slave. */
ec_internal_request_state_t state; /**< Request state. */
unsigned long jiffies_sent; /**< Jiffies, when the upload/download
request was sent. */
uint16_t error_code; /**< SoE error code. */
} ec_soe_request_t;
/*****************************************************************************/
void ec_soe_request_init(ec_soe_request_t *);
void ec_soe_request_clear(ec_soe_request_t *);
void ec_soe_request_set_idn(ec_soe_request_t *, uint16_t);
int ec_soe_request_alloc(ec_soe_request_t *, size_t);
int ec_soe_request_copy_data(ec_soe_request_t *, const uint8_t *, size_t);
void ec_soe_request_read(ec_soe_request_t *);
void ec_soe_request_write(ec_soe_request_t *);
#if 0
int ec_soe_request_copy(ec_soe_request_t *, const ec_soe_request_t *);
int ec_soe_request_timed_out(const ec_soe_request_t *);
#endif
/*****************************************************************************/
#endif

142
tool/CommandSoeRead.cpp Normal file
View File

@ -0,0 +1,142 @@
/*****************************************************************************
*
* $Id$
*
* Copyright (C) 2006-2009 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.
*
****************************************************************************/
#include <iostream>
#include <iomanip>
using namespace std;
#include "CommandSoeRead.h"
#include "MasterDevice.h"
/*****************************************************************************/
CommandSoeRead::CommandSoeRead():
Command("soe_read", "Read an SoE IDN from a slave.")
{
}
/*****************************************************************************/
string CommandSoeRead::helpString() const
{
stringstream str;
str << getName() << " [OPTIONS] <INDEX> <SUBINDEX>" << endl
<< endl
<< getBriefDescription() << endl
<< endl
<< "This command requires a single slave to be selected." << endl
<< endl
<< "Arguments:" << endl
<< " IDN is the IDN and must be an unsigned" << endl
<< " 16 bit number." << endl
<< endl
<< "Command-specific options:" << endl
<< " --alias -a <alias>" << endl
<< " --position -p <pos> Slave selection. See the help of" << endl
<< " the 'slaves' command." << endl
<< endl
<< numericInfo();
return str.str();
}
/****************************************************************************/
void CommandSoeRead::execute(const StringVector &args)
{
SlaveList slaves;
stringstream err, strIdn;
ec_ioctl_slave_soe_t data;
if (args.size() != 1) {
err << "'" << getName() << "' takes one argument!";
throwInvalidUsageException(err);
}
strIdn << args[0];
strIdn
>> resetiosflags(ios::basefield) // guess base from prefix
>> data.idn;
if (strIdn.fail()) {
err << "Invalid IDN '" << args[0] << "'!";
throwInvalidUsageException(err);
}
if (getMasterIndices().size() != 1) {
err << getName() << " requires to select a single master!";
throwInvalidUsageException(err);
}
MasterDevice m(getMasterIndices().front());
m.open(MasterDevice::Read);
slaves = selectedSlaves(m);
if (slaves.size() != 1) {
throwSingleSlaveRequired(slaves.size());
}
data.slave_position = slaves.front().position;
data.mem_size = 1024;
data.data = new uint8_t[data.mem_size + 1];
try {
m.readSoe(&data);
} catch (MasterDeviceSoeException &e) {
delete [] data.data;
err << "CoE read command aborted with code 0x"
<< setfill('0') << hex << setw(4) << e.errorCode;
throwCommandException(err);
} catch (MasterDeviceException &e) {
delete [] data.data;
throw e;
}
m.close();
printRawData(data.data, data.data_size);
delete [] data.data;
}
/****************************************************************************/
void CommandSoeRead::printRawData(
const uint8_t *data,
unsigned int size
)
{
cout << hex << setfill('0');
while (size--) {
cout << "0x" << setw(2) << (unsigned int) *data++;
if (size)
cout << " ";
}
cout << endl;
}
/*****************************************************************************/

52
tool/CommandSoeRead.h Normal file
View File

@ -0,0 +1,52 @@
/*****************************************************************************
*
* $Id$
*
* Copyright (C) 2006-2009 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.
*
****************************************************************************/
#ifndef __COMMANDSOEREAD_H__
#define __COMMANDSOEREAD_H__
#include "Command.h"
/****************************************************************************/
class CommandSoeRead:
public Command
{
public:
CommandSoeRead();
string helpString() const;
void execute(const StringVector &);
protected:
static void printRawData(const uint8_t *, unsigned int);
};
/****************************************************************************/
#endif

2
tool/Makefile.am
View File

@ -56,6 +56,7 @@ ethercat_SOURCES = \
CommandSiiRead.cpp \
CommandSiiWrite.cpp \
CommandSlaves.cpp \
CommandSoeRead.cpp \
CommandStates.cpp \
CommandUpload.cpp \
CommandVersion.cpp \
@ -94,6 +95,7 @@ noinst_HEADERS = \
CommandSiiRead.h \
CommandSiiWrite.h \
CommandSlaves.h \
CommandSoeRead.h \
CommandStates.h \
CommandUpload.h \
CommandVersion.h \

15
tool/MasterDevice.cpp
View File

@ -526,4 +526,19 @@ void MasterDevice::getEoeHandler(
#endif
/****************************************************************************/
void MasterDevice::readSoe(ec_ioctl_slave_soe_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SOE_READ, data) < 0) {
if (errno == EIO && data->error_code) {
throw MasterDeviceSoeException(data->error_code);
} else {
stringstream err;
err << "Failed to read IDN: " << strerror(errno);
throw MasterDeviceException(err);
}
}
}
/*****************************************************************************/

21
tool/MasterDevice.h
View File

@ -67,7 +67,7 @@ class MasterDeviceSdoAbortException:
uint32_t abortCode;
protected:
/** Constructor with stringstream parameter. */
/** Constructor with abort code parameter. */
MasterDeviceSdoAbortException(uint32_t code):
MasterDeviceException("SDO transfer aborted.") {
abortCode = code;
@ -76,6 +76,24 @@ class MasterDeviceSdoAbortException:
/****************************************************************************/
class MasterDeviceSoeException:
public MasterDeviceException
{
friend class MasterDevice;
public:
uint16_t errorCode;
protected:
/** Constructor with error code parameter. */
MasterDeviceSoeException(uint16_t code):
MasterDeviceException("SoE transfer aborted.") {
errorCode = code;
};
};
/****************************************************************************/
class MasterDevice
{
public:
@ -122,6 +140,7 @@ class MasterDevice
#ifdef EC_EOE
void getEoeHandler(ec_ioctl_eoe_handler_t *, uint16_t);
#endif
void readSoe(ec_ioctl_slave_soe_t *);
unsigned int getMasterCount() const {return masterCount;}

2
tool/main.cpp
View File

@ -56,6 +56,7 @@ using namespace std;
#include "CommandSiiRead.h"
#include "CommandSiiWrite.h"
#include "CommandSlaves.h"
#include "CommandSoeRead.h"
#include "CommandStates.h"
#include "CommandUpload.h"
#include "CommandVersion.h"
@ -335,6 +336,7 @@ int main(int argc, char **argv)
commandList.push_back(new CommandSiiRead());
commandList.push_back(new CommandSiiWrite());
commandList.push_back(new CommandSlaves());
commandList.push_back(new CommandSoeRead());
commandList.push_back(new CommandStates());
commandList.push_back(new CommandUpload());
commandList.push_back(new CommandVersion());