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

Implemented VoE handler in userspace.

This commit is contained in:
Florian Pose 2008-10-14 13:35:35 +00:00
parent 494a0ee4ac
commit d31df920e8
8 changed files with 640 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
lib/Makefile.am
View File

@ -43,11 +43,13 @@ libethercat_la_SOURCES = \
common.c \
domain.c \
master.c \
slave_config.c
slave_config.c \
voe_handler.c
noinst_HEADERS = \
domain.h \
master.h \
slave_config.h
slave_config.h \
voe_handler.h
#------------------------------------------------------------------------------

41
lib/slave_config.c
View File

@ -39,6 +39,7 @@
#include "slave_config.h"
#include "domain.h"
#include "voe_handler.h"
#include "master.h"
#include "master/ioctl.h"
@ -306,7 +307,45 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,
ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,
size_t size)
{
return 0; // TODO
ec_ioctl_voe_t data;
ec_voe_handler_t *voe;
unsigned int index;
voe = malloc(sizeof(ec_voe_handler_t));
if (!voe) {
fprintf(stderr, "Failed to allocate memory.\n");
return 0;
}
if (size) {
voe->data = malloc(size);
if (!voe->data) {
fprintf(stderr, "Failed to allocate %u bytes of VoE data"
" memory.\n", size);
free(voe);
return 0;
}
} else {
voe->data = NULL;
}
data.config_index = sc->index;
data.size = size;
if (ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data) == -1) {
fprintf(stderr, "Failed to create VoE handler: %s\n",
strerror(errno));
if (voe->data)
free(voe->data);
free(voe);
return NULL;
}
voe->config = sc;
voe->index = data.voe_index;
voe->data_size = size;
voe->mem_size = size;
return voe;
}
/*****************************************************************************/

163
lib/voe_handler.c Normal file
View File

@ -0,0 +1,163 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 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
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* 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 right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
/** \file
* Vendor-specific-over-EtherCAT protocol handler functions.
*/
/*****************************************************************************/
#include <stdio.h>
#include "voe_handler.h"
#include "slave_config.h"
#include "master.h"
#include "master/ioctl.h"
/*****************************************************************************/
void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
uint16_t vendor_type)
{
ec_ioctl_voe_t data;
data.config_index = voe->config->index;
data.voe_index = voe->index;
data.vendor_id = &vendor_id;
data.vendor_type = &vendor_type;
if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data) == -1) {
fprintf(stderr, "Failed to set VoE send header.\n");
}
}
/*****************************************************************************/
void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
uint32_t *vendor_id, uint16_t *vendor_type)
{
ec_ioctl_voe_t data;
data.config_index = voe->config->index;
data.voe_index = voe->index;
data.vendor_id = vendor_id;
data.vendor_type = vendor_type;
if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data) == -1) {
fprintf(stderr, "Failed to get received VoE header.\n");
}
}
/*****************************************************************************/
uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
{
return voe->data;
}
/*****************************************************************************/
size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)
{
return voe->data_size;
}
/*****************************************************************************/
void ecrt_voe_handler_read(ec_voe_handler_t *voe)
{
ec_ioctl_voe_t data;
data.config_index = voe->config->index;
data.voe_index = voe->index;
if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data) == -1) {
fprintf(stderr, "Failed to initiate VoE reading.\n");
}
}
/*****************************************************************************/
void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
{
ec_ioctl_voe_t data;
data.config_index = voe->config->index;
data.voe_index = voe->index;
data.size = size;
data.data = voe->data;
if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data) == -1) {
fprintf(stderr, "Failed to initiate VoE reading.\n");
}
}
/*****************************************************************************/
ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
{
ec_ioctl_voe_t data;
data.config_index = voe->config->index;
data.voe_index = voe->index;
data.size = 0;
if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data) == -1) {
fprintf(stderr, "Failed to initiate VoE reading.\n");
return EC_REQUEST_ERROR;
}
if (data.size) { // new data waiting to be copied
if (voe->mem_size < data.size) {
if (voe->data)
free(voe->data);
voe->data = malloc(data.size);
if (!voe->data) {
voe->mem_size = 0;
fprintf(stderr, "Failed to allocate VoE data memory!");
return EC_REQUEST_ERROR;
}
voe->mem_size = data.size;
}
if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data) == -1) {
fprintf(stderr, "Failed to get VoE data!\n");
return EC_REQUEST_ERROR;
}
voe->data_size = data.size;
}
return data.state;
}
/*****************************************************************************/

46
lib/voe_handler.h Normal file
View File

@ -0,0 +1,46 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 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
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* 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 right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
#include "include/ecrt.h"
/*****************************************************************************/
struct ec_voe_handler {
ec_slave_config_t *config;
unsigned int index;
size_t data_size;
size_t mem_size;
uint8_t *data;
};
/*****************************************************************************/

344
master/cdev.c
View File

@ -45,6 +45,7 @@
#include "cdev.h"
#include "master.h"
#include "slave_config.h"
#include "voe_handler.h"
#include "ioctl.h"
/*****************************************************************************/
@ -1862,6 +1863,54 @@ int ec_cdev_ioctl_sc_sdo(
/*****************************************************************************/
/** Create a VoE handler.
*/
int ec_cdev_ioctl_sc_create_voe_handler(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
return -EFAULT;
}
data.voe_index = 0;
if (down_interruptible(&master->master_sem))
return -EINTR;
sc = ec_master_get_config(master, data.config_index);
if (!sc) {
up(&master->master_sem);
return -ESRCH;
}
list_for_each_entry(voe, &sc->voe_handlers, list) {
data.voe_index++;
}
up(&master->master_sem);
voe = ecrt_slave_config_create_voe_handler(sc, data.size);
if (!voe)
return -ENOMEM;
if (copy_to_user((void __user *) arg, &data, sizeof(data)))
return -EFAULT;
return 0;
}
/*****************************************************************************/
/** Get the slave configuration's state.
*/
int ec_cdev_ioctl_sc_state(
@ -2026,6 +2075,277 @@ int ec_cdev_ioctl_domain_state(
return 0;
}
/*****************************************************************************/
/** Sets the VoE send header.
*/
int ec_cdev_ioctl_voe_send_header(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
uint32_t vendor_id;
uint16_t vendor_type;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
return -EFAULT;
if (get_user(vendor_id, data.vendor_id))
return -EFAULT;
if (get_user(vendor_type, data.vendor_type))
return -EFAULT;
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(sc = ec_master_get_config(master, data.config_index))) {
up(&master->master_sem);
return -ESRCH;
}
if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
up(&master->master_sem);
return -ESRCH;
}
up(&master->master_sem);
ecrt_voe_handler_send_header(voe, vendor_id, vendor_type);
return 0;
}
/*****************************************************************************/
/** Gets the received VoE header.
*/
int ec_cdev_ioctl_voe_rec_header(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
uint32_t vendor_id;
uint16_t vendor_type;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
return -EFAULT;
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(sc = ec_master_get_config(master, data.config_index))) {
up(&master->master_sem);
return -ESRCH;
}
if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
up(&master->master_sem);
return -ESRCH;
}
ecrt_voe_handler_received_header(voe, &vendor_id, &vendor_type);
up(&master->master_sem);
if (likely(data.vendor_id))
if (put_user(vendor_id, data.vendor_id))
return -EFAULT;
if (likely(data.vendor_type))
if (put_user(vendor_type, data.vendor_type))
return -EFAULT;
return 0;
}
/*****************************************************************************/
/** Starts a VoE read operation.
*/
int ec_cdev_ioctl_voe_read(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
return -EFAULT;
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(sc = ec_master_get_config(master, data.config_index))) {
up(&master->master_sem);
return -ESRCH;
}
if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
up(&master->master_sem);
return -ESRCH;
}
up(&master->master_sem);
ecrt_voe_handler_read(voe);
return 0;
}
/*****************************************************************************/
/** Starts a VoE write operation.
*/
int ec_cdev_ioctl_voe_write(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
return -EFAULT;
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(sc = ec_master_get_config(master, data.config_index))) {
up(&master->master_sem);
return -ESRCH;
}
if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
up(&master->master_sem);
return -ESRCH;
}
up(&master->master_sem);
if (data.size) {
if (data.size > ec_voe_handler_mem_size(voe))
return -EOVERFLOW;
if (copy_from_user(ecrt_voe_handler_data(voe),
(void __user *) data.data, data.size))
return -EFAULT;
}
ecrt_voe_handler_write(voe, data.size);
return 0;
}
/*****************************************************************************/
/** Executes the VoE state machine.
*/
int ec_cdev_ioctl_voe_exec(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
return -EFAULT;
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(sc = ec_master_get_config(master, data.config_index))) {
up(&master->master_sem);
return -ESRCH;
}
if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
up(&master->master_sem);
return -ESRCH;
}
up(&master->master_sem);
data.state = ecrt_voe_handler_execute(voe);
if (data.state == EC_REQUEST_SUCCESS && voe->dir == EC_DIR_INPUT)
data.size = ecrt_voe_handler_data_size(voe);
if (copy_to_user((void __user *) arg, &data, sizeof(data)))
return -EFAULT;
return 0;
}
/*****************************************************************************/
/** Reads the received VoE data.
*/
int ec_cdev_ioctl_voe_data(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg, /**< ioctl() argument. */
ec_cdev_priv_t *priv /**< Private data structure of file handle. */
)
{
ec_ioctl_voe_t data;
ec_slave_config_t *sc;
ec_voe_handler_t *voe;
if (unlikely(!priv->requested))
return -EPERM;
if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
return -EFAULT;
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(sc = ec_master_get_config(master, data.config_index))) {
up(&master->master_sem);
return -ESRCH;
}
if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
up(&master->master_sem);
return -ESRCH;
}
up(&master->master_sem);
if (copy_to_user((void __user *) data.data, ecrt_voe_handler_data(voe),
ecrt_voe_handler_data_size(voe)))
return -EFAULT;
return 0;
}
/******************************************************************************
* File operations
*****************************************************************************/
@ -2198,6 +2518,10 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_sc_sdo(master, arg, priv);
case EC_IOCTL_SC_VOE:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_sc_create_voe_handler(master, arg, priv);
case EC_IOCTL_SC_STATE:
return ec_cdev_ioctl_sc_state(master, arg, priv);
case EC_IOCTL_DOMAIN_OFFSET:
@ -2212,6 +2536,26 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
return ec_cdev_ioctl_domain_queue(master, arg, priv);
case EC_IOCTL_DOMAIN_STATE:
return ec_cdev_ioctl_domain_state(master, arg, priv);
case EC_IOCTL_VOE_SEND_HEADER:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_voe_send_header(master, arg, priv);
case EC_IOCTL_VOE_REC_HEADER:
return ec_cdev_ioctl_voe_rec_header(master, arg, priv);
case EC_IOCTL_VOE_READ:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_voe_read(master, arg, priv);
case EC_IOCTL_VOE_WRITE:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_voe_write(master, arg, priv);
case EC_IOCTL_VOE_EXEC:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_voe_exec(master, arg, priv);
case EC_IOCTL_VOE_DATA:
return ec_cdev_ioctl_voe_data(master, arg, priv);
default:
return -ENOTTY;
}

32
master/ioctl.h
View File

@ -95,11 +95,18 @@
#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x21, ec_ioctl_config_pdo_t)
#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x22, ec_ioctl_reg_pdo_entry_t)
#define EC_IOCTL_SC_SDO EC_IOW(0x23, ec_ioctl_sc_sdo_t)
#define EC_IOCTL_SC_STATE EC_IOWR(0x24, ec_ioctl_sc_state_t)
#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x25)
#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x26)
#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x27)
#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x28, ec_ioctl_domain_state_t)
#define EC_IOCTL_SC_VOE EC_IOWR(0x24, ec_ioctl_voe_t)
#define EC_IOCTL_SC_STATE EC_IOWR(0x25, ec_ioctl_sc_state_t)
#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x26)
#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x27)
#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x28)
#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x29, ec_ioctl_domain_state_t)
#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x2a, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x2b, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_READ EC_IOW(0x2c, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_WRITE EC_IOWR(0x2d, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_EXEC EC_IOWR(0x2e, ec_ioctl_voe_t)
#define EC_IOCTL_VOE_DATA EC_IOWR(0x2f, ec_ioctl_voe_t)
/*****************************************************************************/
@ -440,6 +447,21 @@ typedef struct {
/*****************************************************************************/
typedef struct {
// inputs
uint32_t config_index;
// inputs/outputs
uint32_t voe_index;
uint32_t *vendor_id;
uint16_t *vendor_type;
size_t size;
uint8_t *data;
ec_request_state_t state;
} ec_ioctl_voe_t;
/*****************************************************************************/
/** \endcond */
#endif

15
master/voe_handler.c
View File

@ -107,6 +107,21 @@ void ec_voe_handler_clear(
ec_datagram_clear(&voe->datagram);
}
/*****************************************************************************/
/** Get usable memory size.
*/
size_t ec_voe_handler_mem_size(
const ec_voe_handler_t *voe /**< VoE handler. */
)
{
if (voe->datagram.mem_size >= EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE)
return voe->datagram.mem_size -
(EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
else
return 0;
}
/*****************************************************************************
* Application interface.
****************************************************************************/

1
master/voe_handler.h
View File

@ -70,6 +70,7 @@ struct ec_voe_handler {
int ec_voe_handler_init(ec_voe_handler_t *, ec_slave_config_t *, size_t);
void ec_voe_handler_clear(ec_voe_handler_t *);
size_t ec_voe_handler_mem_size(const ec_voe_handler_t *);
/*****************************************************************************/