fp@1209: /******************************************************************************
fp@1209:  *
fp@1209:  *  $Id$
fp@1209:  *
fp@1209:  *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH
fp@1209:  *
fp@1209:  *  This file is part of the IgH EtherCAT Master.
fp@1209:  *
fp@1209:  *  The IgH EtherCAT Master is free software; you can redistribute it
fp@1209:  *  and/or modify it under the terms of the GNU General Public License
fp@1209:  *  as published by the Free Software Foundation; either version 2 of the
fp@1209:  *  License, or (at your option) any later version.
fp@1209:  *
fp@1209:  *  The IgH EtherCAT Master is distributed in the hope that it will be
fp@1209:  *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
fp@1209:  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
fp@1209:  *  GNU General Public License for more details.
fp@1209:  *
fp@1209:  *  You should have received a copy of the GNU General Public License
fp@1209:  *  along with the IgH EtherCAT Master; if not, write to the Free Software
fp@1209:  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
fp@1209:  *
fp@1209:  *  The right to use EtherCAT Technology is granted and comes free of
fp@1209:  *  charge under condition of compatibility of product made by
fp@1209:  *  Licensee. People intending to distribute/sell products based on the
fp@1209:  *  code, have to sign an agreement to guarantee that products using
fp@1209:  *  software based on IgH EtherCAT master stay compatible with the actual
fp@1209:  *  EtherCAT specification (which are released themselves as an open
fp@1209:  *  standard) as the (only) precondition to have the right to use EtherCAT
fp@1209:  *  Technology, IP and trade marks.
fp@1209:  *
fp@1209:  *****************************************************************************/
fp@1209: 
fp@1209: /** \file
fp@1209:  * Vendor-specific-over-EtherCAT protocol handler functions.
fp@1209:  */
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: #include <linux/module.h>
fp@1209: 
fp@1209: #include "master.h"
fp@1209: #include "slave_config.h"
fp@1209: #include "mailbox.h"
fp@1209: #include "voe_handler.h"
fp@1209: 
fp@1216: /** VoE mailbox type.
fp@1216:  */
fp@1224: #define EC_MBOX_TYPE_VOE 0x0f
fp@1216: 
fp@1218: /** VoE header size.
fp@1218:  */
fp@1218: #define EC_VOE_HEADER_SIZE 6
fp@1218: 
fp@1209: /** VoE response timeout in [ms].
fp@1209:  */
fp@1209: #define EC_VOE_RESPONSE_TIMEOUT 500
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: void ec_voe_handler_state_write_start(ec_voe_handler_t *);
fp@1209: void ec_voe_handler_state_write_response(ec_voe_handler_t *);
fp@1209: 
fp@1209: void ec_voe_handler_state_read_start(ec_voe_handler_t *);
fp@1209: void ec_voe_handler_state_read_check(ec_voe_handler_t *);
fp@1209: void ec_voe_handler_state_read_response(ec_voe_handler_t *);
fp@1209: 
fp@1209: void ec_voe_handler_state_end(ec_voe_handler_t *);
fp@1209: void ec_voe_handler_state_error(ec_voe_handler_t *);
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: /** VoE handler constructor.
fp@1209:  */
fp@1209: int ec_voe_handler_init(
fp@1209:         ec_voe_handler_t *voe, /**< VoE handler. */
fp@1209:         ec_slave_config_t *sc, /**< Parent slave configuration. */
fp@1209:         size_t size /**< Size of memory to reserve. */
fp@1209:         )
fp@1209: {
fp@1209:     voe->config = sc;
fp@1218:     voe->vendor_id = 0x00000000;
fp@1218:     voe->vendor_type = 0x0000;
fp@1209:     voe->data_size = 0;
fp@1209:     voe->dir = EC_DIR_INVALID;
fp@1209:     voe->state = ec_voe_handler_state_error;
fp@1209:     voe->request_state = EC_INT_REQUEST_INIT;
fp@1209: 
fp@1209:     ec_datagram_init(&voe->datagram);
fp@1218:     if (ec_datagram_prealloc(&voe->datagram,
fp@1218:                 size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE))
fp@1209:         return -1;
fp@1209: 
fp@1209:     return 0;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: /** VoE handler destructor.
fp@1209:  */
fp@1209: void ec_voe_handler_clear(
fp@1209:         ec_voe_handler_t *voe /**< VoE handler. */
fp@1209:         )
fp@1209: {
fp@1209:     ec_datagram_clear(&voe->datagram);
fp@1209: }
fp@1209: 
fp@1264: /*****************************************************************************/
fp@1264: 
fp@1264: /** Get usable memory size.
fp@1264:  */
fp@1264: size_t ec_voe_handler_mem_size(
fp@1264: 		const ec_voe_handler_t *voe /**< VoE handler. */
fp@1264: 		)
fp@1264: {
fp@1264: 	if (voe->datagram.mem_size >= EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE)
fp@1264: 		return voe->datagram.mem_size -
fp@1264: 			(EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
fp@1264: 	else
fp@1264: 		return 0;
fp@1264: }
fp@1264: 
fp@1209: /*****************************************************************************
fp@1209:  * Application interface.
fp@1209:  ****************************************************************************/
fp@1209: 
fp@1226: void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
fp@1218:         uint16_t vendor_type)
fp@1218: {
fp@1218:     voe->vendor_id = vendor_id;
fp@1218:     voe->vendor_type = vendor_type;
fp@1218: }
fp@1218: 
fp@1218: /*****************************************************************************/
fp@1218: 
fp@1226: void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
fp@1226:         uint32_t *vendor_id, uint16_t *vendor_type)
fp@1226: {
fp@1226:     uint8_t *header = voe->datagram.data + EC_MBOX_HEADER_SIZE;
fp@1226: 
fp@1226:     if (vendor_id)
fp@1226:         *vendor_id = EC_READ_U32(header);
fp@1226:     if (vendor_type)
fp@1226:         *vendor_type = EC_READ_U16(header + 4);
fp@1226: }
fp@1226: 
fp@1226: /*****************************************************************************/
fp@1226: 
fp@1209: uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
fp@1209: {
fp@1218:     return voe->datagram.data + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     return voe->data_size;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: void ecrt_voe_handler_read(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     voe->dir = EC_DIR_INPUT;
fp@1209:     voe->state = ec_voe_handler_state_read_start;
fp@1209:     voe->request_state = EC_INT_REQUEST_QUEUED;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
fp@1209: {
fp@1209:     voe->dir = EC_DIR_OUTPUT;
fp@1218:     voe->data_size = size;
fp@1209:     voe->state = ec_voe_handler_state_write_start;
fp@1209:     voe->request_state = EC_INT_REQUEST_QUEUED;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     if (voe->config->slave) {
fp@1209:         voe->state(voe);
fp@1209:         if (voe->request_state == EC_REQUEST_BUSY)
fp@1209:             ec_master_queue_datagram(voe->config->master, &voe->datagram);
fp@1209:     } else {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:     }
fp@1209: 
fp@1209:     return ec_request_state_translation_table[voe->request_state];
fp@1209: }
fp@1209: 
fp@1209: /******************************************************************************
fp@1209:  * State functions.
fp@1209:  *****************************************************************************/
fp@1209: 
fp@1268: /** Start writing VoE data.
fp@1268:  */
fp@1209: void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     ec_slave_t *slave = voe->config->slave;
fp@1209:     uint8_t *data;
fp@1209: 
fp@1209:     if (slave->master->debug_level) {
fp@1209:         EC_DBG("Writing %u bytes of VoE data to slave %u.\n",
fp@1209:                voe->data_size, slave->ring_position);
fp@1209:         ec_print_data(ecrt_voe_handler_data(voe), voe->data_size);
fp@1209:     }
fp@1209: 
fp@1209:     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
fp@1209:         EC_ERR("Slave %u does not support VoE!\n", slave->ring_position);
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         return;
fp@1209:     }
fp@1209: 	
fp@1216:     if (!(data = ec_slave_mbox_prepare_send(slave, &voe->datagram,
fp@1218:                     EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size))) {
fp@1218:         voe->state = ec_voe_handler_state_error;
fp@1218:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1218:         return;
fp@1218:     }
fp@1218: 
fp@1218:     EC_WRITE_U32(data,     voe->vendor_id);
fp@1218:     EC_WRITE_U16(data + 4, voe->vendor_type);
fp@1209: 
fp@1209:     voe->retries = EC_FSM_RETRIES;
fp@1209:     voe->jiffies_start = jiffies;
fp@1209:     voe->state = ec_voe_handler_state_write_response;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1268: /** Wait for the mailbox response.
fp@1268:  */
fp@1209: void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     ec_datagram_t *datagram = &voe->datagram;
fp@1209:     ec_slave_t *slave = voe->config->slave;
fp@1209: 
fp@1209:     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
fp@1209:         return;
fp@1209: 
fp@1209:     if (datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_ERR("Failed to receive VoE write request datagram for"
fp@1209:                " slave %u (datagram state %u).\n",
fp@1209:                slave->ring_position, datagram->state);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     if (datagram->working_counter != 1) {
fp@1209:         if (!datagram->working_counter) {
fp@1209:             unsigned long diff_ms =
fp@1209:                 (jiffies - voe->jiffies_start) * 1000 / HZ;
fp@1209:             if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {
fp@1209:                 if (slave->master->debug_level) {
fp@1209:                     EC_DBG("Slave %u did not respond to VoE write request. "
fp@1209:                             "Retrying after %u ms...\n",
fp@1209:                             slave->ring_position, (u32) diff_ms);
fp@1209:                 }
fp@1209:                 // no response; send request datagram again
fp@1209:                 return;
fp@1209:             }
fp@1209:         }
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_ERR("Reception of VoE write request failed on slave %u: ",
fp@1209:                 slave->ring_position);
fp@1209:         ec_datagram_print_wc_error(datagram);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     if (voe->config->master->debug_level)
fp@1209:         EC_DBG("VoE write request successful.\n");
fp@1209: 
fp@1209:     voe->request_state = EC_INT_REQUEST_SUCCESS;
fp@1209:     voe->state = ec_voe_handler_state_end;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1268: /** Start reading VoE data.
fp@1268:  */
fp@1209: void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     ec_datagram_t *datagram = &voe->datagram;
fp@1209:     ec_slave_t *slave = voe->config->slave;
fp@1209: 
fp@1209:     if (slave->master->debug_level)
fp@1209:         EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position);
fp@1209: 
fp@1209:     if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
fp@1209:         EC_ERR("Slave %u does not support VoE!\n", slave->ring_position);
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         return;
fp@1209:     }
fp@1209: 	
fp@1209:     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1209: 
fp@1209:     voe->jiffies_start = jiffies;
fp@1209:     voe->retries = EC_FSM_RETRIES;
fp@1209:     voe->state = ec_voe_handler_state_read_check;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1268: /** Check for new data in the mailbox.
fp@1268:  */
fp@1209: void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     ec_datagram_t *datagram = &voe->datagram;
fp@1209:     ec_slave_t *slave = voe->config->slave;
fp@1209: 
fp@1209:     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
fp@1209:         return;
fp@1209: 
fp@1209:     if (datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_ERR("Failed to receive VoE mailbox check datagram from slave %u"
fp@1209:                 " (datagram state %u).\n",
fp@1209:                slave->ring_position, datagram->state);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     if (datagram->working_counter != 1) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_ERR("Reception of VoE mailbox check"
fp@1209:                 " datagram failed on slave %u: ", slave->ring_position);
fp@1209:         ec_datagram_print_wc_error(datagram);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     if (!ec_slave_mbox_check(datagram)) {
fp@1209:         unsigned long diff_ms =
fp@1209:             (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
fp@1209:         if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
fp@1209:             voe->state = ec_voe_handler_state_error;
fp@1209:             voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:             EC_ERR("Timeout while waiting for VoE data on "
fp@1209:                     "slave %u.\n", slave->ring_position);
fp@1209:             return;
fp@1209:         }
fp@1209: 
fp@1209:         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1209:         voe->retries = EC_FSM_RETRIES;
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     // Fetch response
fp@1209:     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fp@1209:     voe->retries = EC_FSM_RETRIES;
fp@1209:     voe->state = ec_voe_handler_state_read_response;
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1268: /** Read the pending mailbox data.
fp@1268:  */
fp@1209: void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
fp@1209: {
fp@1209:     ec_datagram_t *datagram = &voe->datagram;
fp@1209:     ec_slave_t *slave = voe->config->slave;
fp@1209:     ec_master_t *master = voe->config->master;
fp@1209:     uint8_t *data, mbox_prot;
fp@1209:     size_t rec_size;
fp@1209: 
fp@1209:     if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
fp@1209:         return;
fp@1209: 
fp@1209:     if (datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_ERR("Failed to receive VoE read datagram for"
fp@1209:                " slave %u (datagram state %u).\n",
fp@1209:                slave->ring_position, datagram->state);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     if (datagram->working_counter != 1) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_ERR("Reception of VoE read response failed on slave %u: ",
fp@1209:                 slave->ring_position);
fp@1209:         ec_datagram_print_wc_error(datagram);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1209:     if (!(data = ec_slave_mbox_fetch(slave, datagram,
fp@1209: 				     &mbox_prot, &rec_size))) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1216:     if (mbox_prot != EC_MBOX_TYPE_VOE) {
fp@1209:         voe->state = ec_voe_handler_state_error;
fp@1209:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1209:         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
fp@1209:         ec_print_data(data, rec_size);
fp@1209:         return;
fp@1209:     }
fp@1209: 
fp@1218:     if (rec_size < EC_VOE_HEADER_SIZE) {
fp@1218:         voe->state = ec_voe_handler_state_error;
fp@1218:         voe->request_state = EC_INT_REQUEST_FAILURE;
fp@1218:         EC_ERR("Received VoE header is incomplete (%u bytes)!\n", rec_size);
fp@1218:         return;
fp@1218:     }
fp@1218: 
fp@1209:     if (master->debug_level) {
fp@1209:         EC_DBG("VoE data:\n");
fp@1209:         ec_print_data(data, rec_size);
fp@1209:     }
fp@1209: 
fp@1218:     voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
fp@1209:     voe->request_state = EC_INT_REQUEST_SUCCESS;
fp@1209:     voe->state = ec_voe_handler_state_end; // success
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1268: /** Successful termination state function.
fp@1268:  */
fp@1209: void ec_voe_handler_state_end(ec_voe_handler_t *voe)
fp@1209: {
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1268: /** Failure termination state function.
fp@1268:  */
fp@1209: void ec_voe_handler_state_error(ec_voe_handler_t *voe)
fp@1209: {
fp@1209: }
fp@1209: 
fp@1209: /*****************************************************************************/
fp@1209: 
fp@1209: /** \cond */
fp@1209: 
fp@1226: EXPORT_SYMBOL(ecrt_voe_handler_send_header);
fp@1226: EXPORT_SYMBOL(ecrt_voe_handler_received_header);
fp@1209: EXPORT_SYMBOL(ecrt_voe_handler_data);
fp@1209: EXPORT_SYMBOL(ecrt_voe_handler_data_size);
fp@1209: EXPORT_SYMBOL(ecrt_voe_handler_read);
fp@1209: EXPORT_SYMBOL(ecrt_voe_handler_write);
fp@1209: EXPORT_SYMBOL(ecrt_voe_handler_execute);
fp@1209: 
fp@1209: /** \endcond */
fp@1209: 
fp@1209: /*****************************************************************************/