# HG changeset patch # User Florian Pose # Date 1223991335 0 # Node ID e7882f246d7a5d0d265ec24b73d517d8c58d2fa6 # Parent f44720defbc5cc73bf2079baea29def410786d84 Implemented VoE handler in userspace. diff -r f44720defbc5 -r e7882f246d7a lib/Makefile.am --- a/lib/Makefile.am Tue Oct 14 09:56:38 2008 +0000 +++ b/lib/Makefile.am Tue Oct 14 13:35:35 2008 +0000 @@ -43,11 +43,13 @@ 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 #------------------------------------------------------------------------------ diff -r f44720defbc5 -r e7882f246d7a lib/slave_config.c --- a/lib/slave_config.c Tue Oct 14 09:56:38 2008 +0000 +++ b/lib/slave_config.c Tue Oct 14 13:35:35 2008 +0000 @@ -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_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; } /*****************************************************************************/ diff -r f44720defbc5 -r e7882f246d7a lib/voe_handler.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/voe_handler.c Tue Oct 14 13:35:35 2008 +0000 @@ -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 + +#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; +} + +/*****************************************************************************/ diff -r f44720defbc5 -r e7882f246d7a lib/voe_handler.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/voe_handler.h Tue Oct 14 13:35:35 2008 +0000 @@ -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; +}; + +/*****************************************************************************/ diff -r f44720defbc5 -r e7882f246d7a master/cdev.c --- a/master/cdev.c Tue Oct 14 09:56:38 2008 +0000 +++ b/master/cdev.c Tue Oct 14 13:35:35 2008 +0000 @@ -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 @@ /*****************************************************************************/ +/** 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 @@ 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 @@ 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 @@ 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; } diff -r f44720defbc5 -r e7882f246d7a master/ioctl.h --- a/master/ioctl.h Tue Oct 14 09:56:38 2008 +0000 +++ b/master/ioctl.h Tue Oct 14 13:35:35 2008 +0000 @@ -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 { + // 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 diff -r f44720defbc5 -r e7882f246d7a master/voe_handler.c --- a/master/voe_handler.c Tue Oct 14 09:56:38 2008 +0000 +++ b/master/voe_handler.c Tue Oct 14 13:35:35 2008 +0000 @@ -107,6 +107,21 @@ 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. ****************************************************************************/ diff -r f44720defbc5 -r e7882f246d7a master/voe_handler.h --- a/master/voe_handler.h Tue Oct 14 09:56:38 2008 +0000 +++ b/master/voe_handler.h Tue Oct 14 13:35:35 2008 +0000 @@ -70,6 +70,7 @@ 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 *); /*****************************************************************************/