lib/voe_handler.c
changeset 2589 2b9c78543663
parent 1959 656f114153c2
equal deleted inserted replaced
2415:af21f0bdc7c9 2589:2b9c78543663
     1 /******************************************************************************
     1 /******************************************************************************
     2  *  
     2  *
     3  *  $Id$
     3  *  $Id$
     4  * 
     4  *
     5  *  Copyright (C) 2006-2009  Florian Pose, Ingenieurgemeinschaft IgH
     5  *  Copyright (C) 2006-2012  Florian Pose, Ingenieurgemeinschaft IgH
     6  *  
     6  *
     7  *  This file is part of the IgH EtherCAT master userspace library.
     7  *  This file is part of the IgH EtherCAT master userspace library.
     8  *  
     8  *
     9  *  The IgH EtherCAT master userspace library is free software; you can
     9  *  The IgH EtherCAT master userspace library is free software; you can
    10  *  redistribute it and/or modify it under the terms of the GNU Lesser General
    10  *  redistribute it and/or modify it under the terms of the GNU Lesser General
    11  *  Public License as published by the Free Software Foundation; version 2.1
    11  *  Public License as published by the Free Software Foundation; version 2.1
    12  *  of the License.
    12  *  of the License.
    13  *
    13  *
    17  *  GNU Lesser General Public License for more details.
    17  *  GNU Lesser General Public License for more details.
    18  *
    18  *
    19  *  You should have received a copy of the GNU Lesser General Public License
    19  *  You should have received a copy of the GNU Lesser General Public License
    20  *  along with the IgH EtherCAT master userspace library. If not, see
    20  *  along with the IgH EtherCAT master userspace library. If not, see
    21  *  <http://www.gnu.org/licenses/>.
    21  *  <http://www.gnu.org/licenses/>.
    22  *  
    22  *
    23  *  ---
    23  *  ---
    24  *  
    24  *
    25  *  The license mentioned above concerns the source code only. Using the
    25  *  The license mentioned above concerns the source code only. Using the
    26  *  EtherCAT technology and brand is only permitted in compliance with the
    26  *  EtherCAT technology and brand is only permitted in compliance with the
    27  *  industrial property and similar rights of Beckhoff Automation GmbH.
    27  *  industrial property and similar rights of Beckhoff Automation GmbH.
    28  *
    28  *
    29  *****************************************************************************/
    29  *****************************************************************************/
    33  */
    33  */
    34 
    34 
    35 /*****************************************************************************/
    35 /*****************************************************************************/
    36 
    36 
    37 #include <stdlib.h>
    37 #include <stdlib.h>
    38 #include <sys/ioctl.h>
       
    39 #include <stdio.h>
    38 #include <stdio.h>
    40 #include <errno.h>
       
    41 #include <string.h>
    39 #include <string.h>
    42 
    40 
       
    41 #include "ioctl.h"
    43 #include "voe_handler.h"
    42 #include "voe_handler.h"
    44 #include "slave_config.h"
    43 #include "slave_config.h"
    45 #include "master.h"
    44 #include "master.h"
    46 #include "master/ioctl.h"
       
    47 
    45 
    48 /*****************************************************************************/
    46 /*****************************************************************************/
    49 
    47 
    50 void ec_voe_handler_clear(ec_voe_handler_t *voe)
    48 void ec_voe_handler_clear(ec_voe_handler_t *voe)
    51 {
    49 {
    52     if (voe->data)
    50     if (voe->data) {
    53         free(voe->data);
    51         free(voe->data);
       
    52     }
    54 }
    53 }
    55 
    54 
    56 /*****************************************************************************/
    55 /*****************************************************************************/
    57 
    56 
    58 void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
    57 void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
    59         uint16_t vendor_type)
    58         uint16_t vendor_type)
    60 {
    59 {
    61     ec_ioctl_voe_t data;
    60     ec_ioctl_voe_t data;
       
    61     int ret;
    62 
    62 
    63     data.config_index = voe->config->index;
    63     data.config_index = voe->config->index;
    64     data.voe_index = voe->index;
    64     data.voe_index = voe->index;
    65     data.vendor_id = &vendor_id;
    65     data.vendor_id = &vendor_id;
    66     data.vendor_type = &vendor_type;
    66     data.vendor_type = &vendor_type;
    67 
    67 
    68     if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data) == -1)
    68     ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data);
       
    69     if (EC_IOCTL_IS_ERROR(ret)) {
    69         fprintf(stderr, "Failed to set VoE send header: %s\n",
    70         fprintf(stderr, "Failed to set VoE send header: %s\n",
    70                 strerror(errno));
    71                 strerror(EC_IOCTL_ERRNO(ret)));
       
    72     }
    71 }
    73 }
    72 
    74 
    73 /*****************************************************************************/
    75 /*****************************************************************************/
    74 
    76 
    75 void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
    77 void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
    76         uint32_t *vendor_id, uint16_t *vendor_type)
    78         uint32_t *vendor_id, uint16_t *vendor_type)
    77 {
    79 {
    78     ec_ioctl_voe_t data;
    80     ec_ioctl_voe_t data;
       
    81     int ret;
    79 
    82 
    80     data.config_index = voe->config->index;
    83     data.config_index = voe->config->index;
    81     data.voe_index = voe->index;
    84     data.voe_index = voe->index;
    82     data.vendor_id = vendor_id;
    85     data.vendor_id = vendor_id;
    83     data.vendor_type = vendor_type;
    86     data.vendor_type = vendor_type;
    84 
    87 
    85     if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data) == -1)
    88     ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data);
       
    89     if (EC_IOCTL_IS_ERROR(ret)) {
    86         fprintf(stderr, "Failed to get received VoE header: %s\n",
    90         fprintf(stderr, "Failed to get received VoE header: %s\n",
    87                 strerror(errno));
    91                 strerror(EC_IOCTL_ERRNO(ret)));
       
    92     }
    88 }
    93 }
    89 
    94 
    90 /*****************************************************************************/
    95 /*****************************************************************************/
    91 
    96 
    92 uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
    97 uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
   104 /*****************************************************************************/
   109 /*****************************************************************************/
   105 
   110 
   106 void ecrt_voe_handler_read(ec_voe_handler_t *voe)
   111 void ecrt_voe_handler_read(ec_voe_handler_t *voe)
   107 {
   112 {
   108     ec_ioctl_voe_t data;
   113     ec_ioctl_voe_t data;
   109 
   114     int ret;
   110     data.config_index = voe->config->index;
   115 
   111     data.voe_index = voe->index;
   116     data.config_index = voe->config->index;
   112 
   117     data.voe_index = voe->index;
   113     if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data) == -1)
   118 
       
   119     ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data);
       
   120     if (EC_IOCTL_IS_ERROR(ret)) {
   114         fprintf(stderr, "Failed to initiate VoE reading: %s\n",
   121         fprintf(stderr, "Failed to initiate VoE reading: %s\n",
   115                 strerror(errno));
   122                 strerror(EC_IOCTL_ERRNO(ret)));
       
   123     }
   116 }
   124 }
   117 
   125 
   118 /*****************************************************************************/
   126 /*****************************************************************************/
   119 
   127 
   120 void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)
   128 void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)
   121 {
   129 {
   122     ec_ioctl_voe_t data;
   130     ec_ioctl_voe_t data;
   123 
   131     int ret;
   124     data.config_index = voe->config->index;
   132 
   125     data.voe_index = voe->index;
   133     data.config_index = voe->config->index;
   126 
   134     data.voe_index = voe->index;
   127     if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ_NOSYNC, &data) == -1)
   135 
       
   136     ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ_NOSYNC, &data);
       
   137     if (EC_IOCTL_IS_ERROR(ret)) {
   128         fprintf(stderr, "Failed to initiate VoE reading: %s\n",
   138         fprintf(stderr, "Failed to initiate VoE reading: %s\n",
   129                 strerror(errno));
   139                 strerror(EC_IOCTL_ERRNO(ret)));
       
   140     }
   130 }
   141 }
   131 
   142 
   132 /*****************************************************************************/
   143 /*****************************************************************************/
   133 
   144 
   134 void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
   145 void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
   135 {
   146 {
   136     ec_ioctl_voe_t data;
   147     ec_ioctl_voe_t data;
       
   148     int ret;
   137 
   149 
   138     data.config_index = voe->config->index;
   150     data.config_index = voe->config->index;
   139     data.voe_index = voe->index;
   151     data.voe_index = voe->index;
   140     data.size = size;
   152     data.size = size;
   141     data.data = voe->data;
   153     data.data = voe->data;
   142 
   154 
   143     if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data) == -1)
   155     ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data);
       
   156     if (EC_IOCTL_IS_ERROR(ret)) {
   144         fprintf(stderr, "Failed to initiate VoE writing: %s\n",
   157         fprintf(stderr, "Failed to initiate VoE writing: %s\n",
   145                 strerror(errno));
   158                 strerror(EC_IOCTL_ERRNO(ret)));
       
   159     }
   146 }
   160 }
   147 
   161 
   148 /*****************************************************************************/
   162 /*****************************************************************************/
   149 
   163 
   150 ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
   164 ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
   151 {
   165 {
   152     ec_ioctl_voe_t data;
   166     ec_ioctl_voe_t data;
   153 
   167     int ret;
   154     data.config_index = voe->config->index;
   168 
   155     data.voe_index = voe->index;
   169     data.config_index = voe->config->index;
   156 
   170     data.voe_index = voe->index;
   157     if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data) == -1) {
   171 
       
   172     ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data);
       
   173     if (EC_IOCTL_IS_ERROR(ret)) {
   158         fprintf(stderr, "Failed to execute VoE handler: %s\n",
   174         fprintf(stderr, "Failed to execute VoE handler: %s\n",
   159                 strerror(errno));
   175                 strerror(EC_IOCTL_ERRNO(ret)));
   160         return EC_REQUEST_ERROR;
   176         return EC_REQUEST_ERROR;
   161     }
   177     }
   162 
   178 
   163     if (data.size) { // new data waiting to be copied
   179     if (data.size) { // new data waiting to be copied
   164         if (voe->mem_size < data.size) {
   180         if (voe->mem_size < data.size) {
   165             if (voe->data)
   181             fprintf(stderr, "Received %zu bytes do not fit info VoE data"
   166                 free(voe->data);
   182                     " memory (%zu bytes)!\n", data.size, voe->mem_size);
   167             voe->data = malloc(data.size);
   183             return EC_REQUEST_ERROR;
   168             if (!voe->data) {
       
   169                 voe->mem_size = 0;
       
   170                 fprintf(stderr, "Failed to allocate VoE data memory!");
       
   171                 return EC_REQUEST_ERROR;
       
   172             }
       
   173             voe->mem_size = data.size;
       
   174         }
   184         }
   175 
   185 
   176         data.data = voe->data;
   186         data.data = voe->data;
   177 
   187 
   178         if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data) == -1) {
   188         ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data);
   179             fprintf(stderr, "Failed to get VoE data: %s\n", strerror(errno));
   189         if (EC_IOCTL_IS_ERROR(ret)) {
       
   190             fprintf(stderr, "Failed to get VoE data: %s\n",
       
   191                     strerror(EC_IOCTL_ERRNO(ret)));
   180             return EC_REQUEST_ERROR;
   192             return EC_REQUEST_ERROR;
   181         }
   193         }
   182         voe->data_size = data.size;
   194         voe->data_size = data.size;
   183     }
   195     }
   184 
   196