1 /******************************************************************************

     2  *

     3  *  $Id$

     4  *

     5  *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH

     6  *

     7  *  This file is part of the IgH EtherCAT Master.

     8  *

     9  *  The IgH EtherCAT Master is free software; you can redistribute it

    10  *  and/or modify it under the terms of the GNU General Public License

    11  *  as published by the Free Software Foundation; either version 2 of the

    12  *  License, or (at your option) any later version.

    13  *

    14  *  The IgH EtherCAT Master is distributed in the hope that it will be

    15  *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17  *  GNU General Public License for more details.

    18  *

    19  *  You should have received a copy of the GNU General Public License

    20  *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22  *

    23  *  The right to use EtherCAT Technology is granted and comes free of

    24  *  charge under condition of compatibility of product made by

    25  *  Licensee. People intending to distribute/sell products based on the

    26  *  code, have to sign an agreement to guarantee that products using

    27  *  software based on IgH EtherCAT master stay compatible with the actual

    28  *  EtherCAT specification (which are released themselves as an open

    29  *  standard) as the (only) precondition to have the right to use EtherCAT

    30  *  Technology, IP and trade marks.

    31  *

    32  *****************************************************************************/

    33 

    34 /** \file

    35  * Vendor-specific-over-EtherCAT protocol handler functions.

    36  */

    37 

    38 /*****************************************************************************/

    39 

    40 #include <linux/module.h>

    41 

    42 #include "master.h"

    43 #include "slave_config.h"

    44 #include "mailbox.h"

    45 #include "voe_handler.h"

    46 

    47 /** VoE response timeout in [ms].

    48  */

    49 #define EC_VOE_RESPONSE_TIMEOUT 500

    50 

    51 /*****************************************************************************/

    52 

    53 void ec_voe_handler_clear_data(ec_voe_handler_t *);

    54 

    55 void ec_voe_handler_state_write_start(ec_voe_handler_t *);

    56 void ec_voe_handler_state_write_response(ec_voe_handler_t *);

    57 

    58 void ec_voe_handler_state_read_start(ec_voe_handler_t *);

    59 void ec_voe_handler_state_read_check(ec_voe_handler_t *);

    60 void ec_voe_handler_state_read_response(ec_voe_handler_t *);

    61 

    62 void ec_voe_handler_state_end(ec_voe_handler_t *);

    63 void ec_voe_handler_state_error(ec_voe_handler_t *);

    64 

    65 /*****************************************************************************/

    66 

    67 /** VoE handler constructor.

    68  */

    69 int ec_voe_handler_init(

    70         ec_voe_handler_t *voe, /**< VoE handler. */

    71         ec_slave_config_t *sc, /**< Parent slave configuration. */

    72         size_t size /**< Size of memory to reserve. */

    73         )

    74 {

    75     voe->config = sc;

    76     voe->data_size = 0;

    77     voe->dir = EC_DIR_INVALID;

    78     voe->state = ec_voe_handler_state_error;

    79     voe->request_state = EC_INT_REQUEST_INIT;

    80 

    81     ec_datagram_init(&voe->datagram);

    82     if (ec_datagram_prealloc(&voe->datagram, size + 6))

    83         return -1;

    84 

    85     return 0;

    86 }

    87 

    88 /*****************************************************************************/

    89 

    90 /** VoE handler destructor.

    91  */

    92 void ec_voe_handler_clear(

    93         ec_voe_handler_t *voe /**< VoE handler. */

    94         )

    95 {

    96     ec_datagram_clear(&voe->datagram);

    97 }

    98 

    99 /*****************************************************************************

   100  * Application interface.

   101  ****************************************************************************/

   102 

   103 uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   104 {

   105     return voe->datagram.data + 6;

   106 }

   107 

   108 /*****************************************************************************/

   109 

   110 size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   111 {

   112     return voe->data_size;

   113 }

   114 

   115 /*****************************************************************************/

   116 

   117 void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   118 {

   119     voe->dir = EC_DIR_INPUT;

   120     voe->state = ec_voe_handler_state_read_start;

   121     voe->request_state = EC_INT_REQUEST_QUEUED;

   122 }

   123 

   124 /*****************************************************************************/

   125 

   126 void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   127 {

   128     voe->dir = EC_DIR_OUTPUT;

   129     voe->datagram.data_size = size + 6;

   130     voe->state = ec_voe_handler_state_write_start;

   131     voe->request_state = EC_INT_REQUEST_QUEUED;

   132 }

   133 

   134 /*****************************************************************************/

   135 

   136 ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   137 {

   138     if (voe->config->slave) {

   139         voe->state(voe);

   140         if (voe->request_state == EC_REQUEST_BUSY)

   141             ec_master_queue_datagram(voe->config->master, &voe->datagram);

   142     } else {

   143         voe->state = ec_voe_handler_state_error;

   144         voe->request_state = EC_INT_REQUEST_FAILURE;

   145     }

   146 

   147     return ec_request_state_translation_table[voe->request_state];

   148 }

   149 

   150 /******************************************************************************

   151  * State functions.

   152  *****************************************************************************/

   153 

   154 void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   155 {

   156     ec_slave_t *slave = voe->config->slave;

   157     uint8_t *data;

   158 

   159     if (slave->master->debug_level) {

   160         EC_DBG("Writing %u bytes of VoE data to slave %u.\n",

   161                voe->data_size, slave->ring_position);

   162         ec_print_data(ecrt_voe_handler_data(voe), voe->data_size);

   163     }

   164 

   165     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {

   166         EC_ERR("Slave %u does not support VoE!\n", slave->ring_position);

   167         voe->state = ec_voe_handler_state_error;

   168         voe->request_state = EC_INT_REQUEST_FAILURE;

   169         return;

   170     }

   171 	

   172     if (!(data = ec_slave_mbox_prepare_send(

   173                     slave, &voe->datagram, 0x01, voe->data_size))) {

   174         voe->state = ec_voe_handler_state_error;

   175         voe->request_state = EC_INT_REQUEST_FAILURE;

   176         return;

   177     }

   178 

   179     voe->retries = EC_FSM_RETRIES;

   180     voe->jiffies_start = jiffies;

   181     voe->state = ec_voe_handler_state_write_response;

   182 }

   183 

   184 /*****************************************************************************/

   185 

   186 void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   187 {

   188     ec_datagram_t *datagram = &voe->datagram;

   189     ec_slave_t *slave = voe->config->slave;

   190 

   191     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)

   192         return;

   193 

   194     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   195         voe->state = ec_voe_handler_state_error;

   196         voe->request_state = EC_INT_REQUEST_FAILURE;

   197         EC_ERR("Failed to receive VoE write request datagram for"

   198                " slave %u (datagram state %u).\n",

   199                slave->ring_position, datagram->state);

   200         return;

   201     }

   202 

   203     if (datagram->working_counter != 1) {

   204         if (!datagram->working_counter) {

   205             unsigned long diff_ms =

   206                 (jiffies - voe->jiffies_start) * 1000 / HZ;

   207             if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   208                 if (slave->master->debug_level) {

   209                     EC_DBG("Slave %u did not respond to VoE write request. "

   210                             "Retrying after %u ms...\n",

   211                             slave->ring_position, (u32) diff_ms);

   212                 }

   213                 // no response; send request datagram again

   214                 return;

   215             }

   216         }

   217         voe->state = ec_voe_handler_state_error;

   218         voe->request_state = EC_INT_REQUEST_FAILURE;

   219         EC_ERR("Reception of VoE write request failed on slave %u: ",

   220                 slave->ring_position);

   221         ec_datagram_print_wc_error(datagram);

   222         return;

   223     }

   224 

   225     if (voe->config->master->debug_level)

   226         EC_DBG("VoE write request successful.\n");

   227 

   228     voe->request_state = EC_INT_REQUEST_SUCCESS;

   229     voe->state = ec_voe_handler_state_end;

   230 }

   231 

   232 /*****************************************************************************/

   233 

   234 void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   235 {

   236     ec_datagram_t *datagram = &voe->datagram;

   237     ec_slave_t *slave = voe->config->slave;

   238 

   239     if (slave->master->debug_level)

   240         EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position);

   241 

   242     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {

   243         EC_ERR("Slave %u does not support VoE!\n", slave->ring_position);

   244         voe->state = ec_voe_handler_state_error;

   245         voe->request_state = EC_INT_REQUEST_FAILURE;

   246         return;

   247     }

   248 	

   249     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   250 

   251     voe->jiffies_start = jiffies;

   252     voe->retries = EC_FSM_RETRIES;

   253     voe->state = ec_voe_handler_state_read_check;

   254 }

   255 

   256 /*****************************************************************************/

   257 

   258 void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   259 {

   260     ec_datagram_t *datagram = &voe->datagram;

   261     ec_slave_t *slave = voe->config->slave;

   262 

   263     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)

   264         return;

   265 

   266     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   267         voe->state = ec_voe_handler_state_error;

   268         voe->request_state = EC_INT_REQUEST_FAILURE;

   269         EC_ERR("Failed to receive VoE mailbox check datagram from slave %u"

   270                 " (datagram state %u).\n",

   271                slave->ring_position, datagram->state);

   272         return;

   273     }

   274 

   275     if (datagram->working_counter != 1) {

   276         voe->state = ec_voe_handler_state_error;

   277         voe->request_state = EC_INT_REQUEST_FAILURE;

   278         EC_ERR("Reception of VoE mailbox check"

   279                 " datagram failed on slave %u: ", slave->ring_position);

   280         ec_datagram_print_wc_error(datagram);

   281         return;

   282     }

   283 

   284     if (!ec_slave_mbox_check(datagram)) {

   285         unsigned long diff_ms =

   286             (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;

   287         if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   288             voe->state = ec_voe_handler_state_error;

   289             voe->request_state = EC_INT_REQUEST_FAILURE;

   290             EC_ERR("Timeout while waiting for VoE data on "

   291                     "slave %u.\n", slave->ring_position);

   292             return;

   293         }

   294 

   295         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   296         voe->retries = EC_FSM_RETRIES;

   297         return;

   298     }

   299 

   300     // Fetch response

   301     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   302     voe->retries = EC_FSM_RETRIES;

   303     voe->state = ec_voe_handler_state_read_response;

   304 }

   305 

   306 /*****************************************************************************/

   307 

   308 void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   309 {

   310     ec_datagram_t *datagram = &voe->datagram;

   311     ec_slave_t *slave = voe->config->slave;

   312     ec_master_t *master = voe->config->master;

   313     uint8_t *data, mbox_prot;

   314     size_t rec_size;

   315 

   316     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)

   317         return;

   318 

   319     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   320         voe->state = ec_voe_handler_state_error;

   321         voe->request_state = EC_INT_REQUEST_FAILURE;

   322         EC_ERR("Failed to receive VoE read datagram for"

   323                " slave %u (datagram state %u).\n",

   324                slave->ring_position, datagram->state);

   325         return;

   326     }

   327 

   328     if (datagram->working_counter != 1) {

   329         voe->state = ec_voe_handler_state_error;

   330         voe->request_state = EC_INT_REQUEST_FAILURE;

   331         EC_ERR("Reception of VoE read response failed on slave %u: ",

   332                 slave->ring_position);

   333         ec_datagram_print_wc_error(datagram);

   334         return;

   335     }

   336 

   337     if (!(data = ec_slave_mbox_fetch(slave, datagram,

   338 				     &mbox_prot, &rec_size))) {

   339         voe->state = ec_voe_handler_state_error;

   340         voe->request_state = EC_INT_REQUEST_FAILURE;

   341         return;

   342     }

   343 

   344     if (mbox_prot != 0x01) { // VoE

   345         voe->state = ec_voe_handler_state_error;

   346         voe->request_state = EC_INT_REQUEST_FAILURE;

   347         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   348         ec_print_data(data, rec_size);

   349         return;

   350     }

   351 

   352     if (master->debug_level) {

   353         EC_DBG("VoE data:\n");

   354         ec_print_data(data, rec_size);

   355     }

   356 

   357     voe->data_size = rec_size;

   358     voe->request_state = EC_INT_REQUEST_SUCCESS;

   359     voe->state = ec_voe_handler_state_end; // success

   360 }

   361 

   362 /*****************************************************************************/

   363 

   364 void ec_voe_handler_state_end(ec_voe_handler_t *voe)

   365 {

   366 }

   367 

   368 /*****************************************************************************/

   369 

   370 void ec_voe_handler_state_error(ec_voe_handler_t *voe)

   371 {

   372 }

   373 

   374 /*****************************************************************************/

   375 

   376 /** \cond */

   377 

   378 EXPORT_SYMBOL(ecrt_voe_handler_data);

   379 EXPORT_SYMBOL(ecrt_voe_handler_data_size);

   380 EXPORT_SYMBOL(ecrt_voe_handler_read);

   381 EXPORT_SYMBOL(ecrt_voe_handler_write);

   382 EXPORT_SYMBOL(ecrt_voe_handler_execute);

   383 

   384 /** \endcond */

   385 

   386 /*****************************************************************************/