IgH EtherCAT Master: voe_handler.c Source File

00001 /******************************************************************************
00002  *
00003  *  $Id$
00004  *
00005  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
00006  *
00007  *  This file is part of the IgH EtherCAT Master.
00008  *
00009  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
00010  *  modify it under the terms of the GNU General Public License version 2, as
00011  *  published by the Free Software Foundation.
00012  *
00013  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
00016  *  Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License along
00019  *  with the IgH EtherCAT Master; if not, write to the Free Software
00020  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  *  ---
00023  *
00024  *  The license mentioned above concerns the source code only. Using the
00025  *  EtherCAT technology and brand is only permitted in compliance with the
00026  *  industrial property and similar rights of Beckhoff Automation GmbH.
00027  *
00028  *****************************************************************************/
00029 
00034 /*****************************************************************************/
00035 
00036 #include <linux/module.h>
00037 
00038 #include "master.h"
00039 #include "slave_config.h"
00040 #include "mailbox.h"
00041 #include "voe_handler.h"
00042 
00045 #define EC_MBOX_TYPE_VOE 0x0f
00046 
00049 #define EC_VOE_HEADER_SIZE 6
00050 
00053 #define EC_VOE_RESPONSE_TIMEOUT 500
00054 
00055 /*****************************************************************************/
00056 
00057 void ec_voe_handler_state_write_start(ec_voe_handler_t *);
00058 void ec_voe_handler_state_write_response(ec_voe_handler_t *);
00059 
00060 void ec_voe_handler_state_read_start(ec_voe_handler_t *);
00061 void ec_voe_handler_state_read_check(ec_voe_handler_t *);
00062 void ec_voe_handler_state_read_response(ec_voe_handler_t *);
00063 
00064 void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);
00065 void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);
00066 
00067 void ec_voe_handler_state_end(ec_voe_handler_t *);
00068 void ec_voe_handler_state_error(ec_voe_handler_t *);
00069 
00070 /*****************************************************************************/
00071 
00076 int ec_voe_handler_init(
00077         ec_voe_handler_t *voe, 
00078         ec_slave_config_t *sc, 
00079         size_t size 
00080         )
00081 {
00082     voe->config = sc;
00083     voe->vendor_id = 0x00000000;
00084     voe->vendor_type = 0x0000;
00085     voe->data_size = 0;
00086     voe->dir = EC_DIR_INVALID;
00087     voe->state = ec_voe_handler_state_error;
00088     voe->request_state = EC_INT_REQUEST_INIT;
00089 
00090     ec_datagram_init(&voe->datagram);
00091     return ec_datagram_prealloc(&voe->datagram,
00092             size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
00093 }
00094 
00095 /*****************************************************************************/
00096 
00099 void ec_voe_handler_clear(
00100         ec_voe_handler_t *voe 
00101         )
00102 {
00103     ec_datagram_clear(&voe->datagram);
00104 }
00105 
00106 /*****************************************************************************/
00107 
00112 size_t ec_voe_handler_mem_size(
00113         const ec_voe_handler_t *voe 
00114         )
00115 {
00116     if (voe->datagram.mem_size >= EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE)
00117         return voe->datagram.mem_size -
00118             (EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
00119     else
00120         return 0;
00121 }
00122 
00123 /*****************************************************************************
00124  * Application interface.
00125  ****************************************************************************/
00126 
00127 void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
00128         uint16_t vendor_type)
00129 {
00130     voe->vendor_id = vendor_id;
00131     voe->vendor_type = vendor_type;
00132 }
00133 
00134 /*****************************************************************************/
00135 
00136 void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
00137         uint32_t *vendor_id, uint16_t *vendor_type)
00138 {
00139     uint8_t *header = voe->datagram.data + EC_MBOX_HEADER_SIZE;
00140 
00141     if (vendor_id)
00142         *vendor_id = EC_READ_U32(header);
00143     if (vendor_type)
00144         *vendor_type = EC_READ_U16(header + 4);
00145 }
00146 
00147 /*****************************************************************************/
00148 
00149 uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
00150 {
00151     return voe->datagram.data + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE;
00152 }
00153 
00154 /*****************************************************************************/
00155 
00156 size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)
00157 {
00158     return voe->data_size;
00159 }
00160 
00161 /*****************************************************************************/
00162 
00163 void ecrt_voe_handler_read(ec_voe_handler_t *voe)
00164 {
00165     voe->dir = EC_DIR_INPUT;
00166     voe->state = ec_voe_handler_state_read_start;
00167     voe->request_state = EC_INT_REQUEST_BUSY;
00168 }
00169 
00170 /*****************************************************************************/
00171 
00172 void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)
00173 {
00174     voe->dir = EC_DIR_INPUT;
00175     voe->state = ec_voe_handler_state_read_nosync_start;
00176     voe->request_state = EC_INT_REQUEST_BUSY;
00177 }
00178 
00179 /*****************************************************************************/
00180 
00181 void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
00182 {
00183     voe->dir = EC_DIR_OUTPUT;
00184     voe->data_size = size;
00185     voe->state = ec_voe_handler_state_write_start;
00186     voe->request_state = EC_INT_REQUEST_BUSY;
00187 }
00188 
00189 /*****************************************************************************/
00190 
00191 ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
00192 {
00193     if (voe->config->slave) { // FIXME locking?
00194         voe->state(voe);
00195         if (voe->request_state == EC_INT_REQUEST_BUSY) {
00196             ec_master_queue_datagram(voe->config->master, &voe->datagram);
00197         }
00198     } else {
00199         voe->state = ec_voe_handler_state_error;
00200         voe->request_state = EC_INT_REQUEST_FAILURE;
00201     }
00202 
00203     return ec_request_state_translation_table[voe->request_state];
00204 }
00205 
00206 /******************************************************************************
00207  * State functions.
00208  *****************************************************************************/
00209 
00212 void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
00213 {
00214     ec_slave_t *slave = voe->config->slave;
00215     uint8_t *data;
00216 
00217     if (slave->master->debug_level) {
00218         EC_SLAVE_DBG(slave, 0, "Writing %zu bytes of VoE data.\n",
00219                voe->data_size);
00220         ec_print_data(ecrt_voe_handler_data(voe), voe->data_size);
00221     }
00222 
00223     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
00224         EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");
00225         voe->state = ec_voe_handler_state_error;
00226         voe->request_state = EC_INT_REQUEST_FAILURE;
00227         return;
00228     }
00229 
00230     data = ec_slave_mbox_prepare_send(slave, &voe->datagram,
00231             EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);
00232     if (IS_ERR(data)) {
00233         voe->state = ec_voe_handler_state_error;
00234         voe->request_state = EC_INT_REQUEST_FAILURE;
00235         return;
00236     }
00237 
00238     EC_WRITE_U32(data,     voe->vendor_id);
00239     EC_WRITE_U16(data + 4, voe->vendor_type);
00240     /* data already in datagram */
00241 
00242     voe->retries = EC_FSM_RETRIES;
00243     voe->jiffies_start = jiffies;
00244     voe->state = ec_voe_handler_state_write_response;
00245 }
00246 
00247 /*****************************************************************************/
00248 
00251 void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
00252 {
00253     ec_datagram_t *datagram = &voe->datagram;
00254     ec_slave_t *slave = voe->config->slave;
00255 
00256     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
00257         return;
00258 
00259     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00260         voe->state = ec_voe_handler_state_error;
00261         voe->request_state = EC_INT_REQUEST_FAILURE;
00262         EC_SLAVE_ERR(slave, "Failed to receive VoE write request datagram: ");
00263         ec_datagram_print_state(datagram);
00264         return;
00265     }
00266 
00267     if (datagram->working_counter != 1) {
00268         if (!datagram->working_counter) {
00269             unsigned long diff_ms =
00270                 (jiffies - voe->jiffies_start) * 1000 / HZ;
00271             if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {
00272                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
00273                         " VoE write request. Retrying after %lu ms...\n",
00274                         diff_ms);
00275                 // no response; send request datagram again
00276                 return;
00277             }
00278         }
00279         voe->state = ec_voe_handler_state_error;
00280         voe->request_state = EC_INT_REQUEST_FAILURE;
00281         EC_SLAVE_ERR(slave, "Reception of VoE write request failed: ");
00282         ec_datagram_print_wc_error(datagram);
00283         return;
00284     }
00285 
00286     EC_CONFIG_DBG(voe->config, 1, "VoE write request successful.\n");
00287 
00288     voe->request_state = EC_INT_REQUEST_SUCCESS;
00289     voe->state = ec_voe_handler_state_end;
00290 }
00291 
00292 /*****************************************************************************/
00293 
00296 void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
00297 {
00298     ec_datagram_t *datagram = &voe->datagram;
00299     ec_slave_t *slave = voe->config->slave;
00300 
00301     EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");
00302 
00303     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
00304         EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");
00305         voe->state = ec_voe_handler_state_error;
00306         voe->request_state = EC_INT_REQUEST_FAILURE;
00307         return;
00308     }
00309 
00310     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
00311 
00312     voe->jiffies_start = jiffies;
00313     voe->retries = EC_FSM_RETRIES;
00314     voe->state = ec_voe_handler_state_read_check;
00315 }
00316 
00317 /*****************************************************************************/
00318 
00321 void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
00322 {
00323     ec_datagram_t *datagram = &voe->datagram;
00324     ec_slave_t *slave = voe->config->slave;
00325 
00326     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
00327         return;
00328 
00329     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00330         voe->state = ec_voe_handler_state_error;
00331         voe->request_state = EC_INT_REQUEST_FAILURE;
00332         EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: ");
00333         ec_datagram_print_state(datagram);
00334         return;
00335     }
00336 
00337     if (datagram->working_counter != 1) {
00338         voe->state = ec_voe_handler_state_error;
00339         voe->request_state = EC_INT_REQUEST_FAILURE;
00340         EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"
00341                 " datagram failed: ");
00342         ec_datagram_print_wc_error(datagram);
00343         return;
00344     }
00345 
00346     if (!ec_slave_mbox_check(datagram)) {
00347         unsigned long diff_ms =
00348             (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
00349         if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
00350             voe->state = ec_voe_handler_state_error;
00351             voe->request_state = EC_INT_REQUEST_FAILURE;
00352             EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n");
00353             return;
00354         }
00355 
00356         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
00357         voe->retries = EC_FSM_RETRIES;
00358         return;
00359     }
00360 
00361     // Fetch response
00362     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
00363     voe->retries = EC_FSM_RETRIES;
00364     voe->state = ec_voe_handler_state_read_response;
00365 }
00366 
00367 /*****************************************************************************/
00368 
00371 void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
00372 {
00373     ec_datagram_t *datagram = &voe->datagram;
00374     ec_slave_t *slave = voe->config->slave;
00375     ec_master_t *master = voe->config->master;
00376     uint8_t *data, mbox_prot;
00377     size_t rec_size;
00378 
00379     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
00380         return;
00381 
00382     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00383         voe->state = ec_voe_handler_state_error;
00384         voe->request_state = EC_INT_REQUEST_FAILURE;
00385         EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
00386         ec_datagram_print_state(datagram);
00387         return;
00388     }
00389 
00390     if (datagram->working_counter != 1) {
00391         voe->state = ec_voe_handler_state_error;
00392         voe->request_state = EC_INT_REQUEST_FAILURE;
00393         EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");
00394         ec_datagram_print_wc_error(datagram);
00395         return;
00396     }
00397 
00398     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
00399     if (IS_ERR(data)) {
00400         voe->state = ec_voe_handler_state_error;
00401         voe->request_state = EC_INT_REQUEST_FAILURE;
00402         return;
00403     }
00404 
00405     if (mbox_prot != EC_MBOX_TYPE_VOE) {
00406         voe->state = ec_voe_handler_state_error;
00407         voe->request_state = EC_INT_REQUEST_FAILURE;
00408         EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"
00409                 " as response.\n", mbox_prot);
00410         ec_print_data(data, rec_size);
00411         return;
00412     }
00413 
00414     if (rec_size < EC_VOE_HEADER_SIZE) {
00415         voe->state = ec_voe_handler_state_error;
00416         voe->request_state = EC_INT_REQUEST_FAILURE;
00417         EC_SLAVE_ERR(slave, "Received VoE header is"
00418                 " incomplete (%zu bytes)!\n", rec_size);
00419         return;
00420     }
00421 
00422     if (master->debug_level) {
00423         EC_CONFIG_DBG(voe->config, 0, "VoE data:\n");
00424         ec_print_data(data, rec_size);
00425     }
00426 
00427     voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
00428     voe->request_state = EC_INT_REQUEST_SUCCESS;
00429     voe->state = ec_voe_handler_state_end; // success
00430 }
00431 
00432 /*****************************************************************************/
00433 
00436 void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)
00437 {
00438     ec_datagram_t *datagram = &voe->datagram;
00439     ec_slave_t *slave = voe->config->slave;
00440 
00441     EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");
00442 
00443     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
00444         EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");
00445         voe->state = ec_voe_handler_state_error;
00446         voe->request_state = EC_INT_REQUEST_FAILURE;
00447         return;
00448     }
00449 
00450     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
00451 
00452     voe->jiffies_start = jiffies;
00453     voe->retries = EC_FSM_RETRIES;
00454     voe->state = ec_voe_handler_state_read_nosync_response;
00455 }
00456 
00457 /*****************************************************************************/
00458 
00462 void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
00463 {
00464     ec_datagram_t *datagram = &voe->datagram;
00465     ec_slave_t *slave = voe->config->slave;
00466     ec_master_t *master = voe->config->master;
00467     uint8_t *data, mbox_prot;
00468     size_t rec_size;
00469 
00470     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
00471         return;
00472 
00473     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00474         voe->state = ec_voe_handler_state_error;
00475         voe->request_state = EC_INT_REQUEST_FAILURE;
00476         EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
00477         ec_datagram_print_state(datagram);
00478         return;
00479     }
00480 
00481     if (datagram->working_counter == 0) {
00482         voe->state = ec_voe_handler_state_error;
00483         voe->request_state = EC_INT_REQUEST_FAILURE;
00484         EC_SLAVE_DBG(slave, 1, "Slave did not send VoE data.\n");
00485         return;
00486     }
00487 
00488     if (datagram->working_counter != 1) {
00489         voe->state = ec_voe_handler_state_error;
00490         voe->request_state = EC_INT_REQUEST_FAILURE;
00491         EC_SLAVE_WARN(slave, "Reception of VoE read response failed: ");
00492         ec_datagram_print_wc_error(datagram);
00493         return;
00494     }
00495 
00496     if (!(data = ec_slave_mbox_fetch(slave, datagram,
00497                     &mbox_prot, &rec_size))) {
00498         voe->state = ec_voe_handler_state_error;
00499         voe->request_state = EC_INT_REQUEST_FAILURE;
00500         return;
00501     }
00502 
00503     if (mbox_prot != EC_MBOX_TYPE_VOE) {
00504         voe->state = ec_voe_handler_state_error;
00505         voe->request_state = EC_INT_REQUEST_FAILURE;
00506         EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"
00507                 " as response.\n", mbox_prot);
00508         ec_print_data(data, rec_size);
00509         return;
00510     }
00511 
00512     if (rec_size < EC_VOE_HEADER_SIZE) {
00513         voe->state = ec_voe_handler_state_error;
00514         voe->request_state = EC_INT_REQUEST_FAILURE;
00515         EC_SLAVE_ERR(slave, "Received VoE header is"
00516                 " incomplete (%zu bytes)!\n", rec_size);
00517         return;
00518     }
00519 
00520     if (master->debug_level) {
00521         EC_CONFIG_DBG(voe->config, 1, "VoE data:\n");
00522         ec_print_data(data, rec_size);
00523     }
00524 
00525     voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
00526     voe->request_state = EC_INT_REQUEST_SUCCESS;
00527     voe->state = ec_voe_handler_state_end; // success
00528 }
00529 
00530 /*****************************************************************************/
00531 
00534 void ec_voe_handler_state_end(ec_voe_handler_t *voe)
00535 {
00536 }
00537 
00538 /*****************************************************************************/
00539 
00542 void ec_voe_handler_state_error(ec_voe_handler_t *voe)
00543 {
00544 }
00545 
00546 /*****************************************************************************/
00547 
00550 EXPORT_SYMBOL(ecrt_voe_handler_send_header);
00551 EXPORT_SYMBOL(ecrt_voe_handler_received_header);
00552 EXPORT_SYMBOL(ecrt_voe_handler_data);
00553 EXPORT_SYMBOL(ecrt_voe_handler_data_size);
00554 EXPORT_SYMBOL(ecrt_voe_handler_read);
00555 EXPORT_SYMBOL(ecrt_voe_handler_write);
00556 EXPORT_SYMBOL(ecrt_voe_handler_execute);
00557 
00560 /*****************************************************************************/