fp@1831: /******************************************************************************
fp@1831:  *
fp@1831:  *  $Id$
fp@1831:  *
fp@1831:  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
fp@1831:  *
fp@1831:  *  This file is part of the IgH EtherCAT Master.
fp@1831:  *
fp@1831:  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
fp@1831:  *  modify it under the terms of the GNU General Public License version 2, as
fp@1831:  *  published by the Free Software Foundation.
fp@1831:  *
fp@1831:  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
fp@1831:  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
fp@1831:  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
fp@1831:  *  Public License for more details.
fp@1831:  *
fp@1831:  *  You should have received a copy of the GNU General Public License along
fp@1831:  *  with the IgH EtherCAT Master; if not, write to the Free Software
fp@1831:  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
fp@1831:  *
fp@1831:  *  ---
fp@1831:  *
fp@1831:  *  The license mentioned above concerns the source code only. Using the
fp@1831:  *  EtherCAT technology and brand is only permitted in compliance with the
fp@1831:  *  industrial property and similar rights of Beckhoff Automation GmbH.
fp@1831:  *
fp@1831:  *****************************************************************************/
fp@1831: 
fp@1831: /**
fp@1831:    \file
fp@1831:    EtherCAT SoE state machines.
fp@1831: */
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: #include "globals.h"
fp@1831: #include "master.h"
fp@1831: #include "mailbox.h"
fp@1831: #include "fsm_soe.h"
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** Mailbox type for SoE.
fp@1831:  */
fp@1831: #define EC_MBOX_TYPE_SOE 0x05
fp@1831: 
fp@1905: /** SoE operations
fp@1905:  */
fp@1905: enum ec_soe_opcodes {
fp@1905:     OPCODE_READ_REQUEST   = 0x01, /**< Read request. */
fp@1905:     OPCODE_READ_RESPONSE  = 0x02, /**< Read response. */
fp@1905:     OPCODE_WRITE_REQUEST  = 0x03, /**< Write request. */
fp@1905:     OPCODE_WRITE_RESPONSE = 0x04  /**< Write response. */
fp@1905: };
fp@1905: 
fp@1905: /** Size of all SoE headers.
fp@1905:  */
fp@1905: #define EC_SOE_SIZE 0x04
fp@1905: 
fp@1905: /** SoE response timeout [ms].
fp@1905:  */
fp@1831: #define EC_SOE_RESPONSE_TIMEOUT 1000
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@2589: void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: 
fp@2589: void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: 
fp@2589: void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
fp@2589: void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1877: extern const ec_code_msg_t soe_error_codes[];
fp@1877: 
fp@1877: /*****************************************************************************/
fp@1877: 
fp@1877: /** Outputs an SoE error code.
fp@1877: */
fp@1921: void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
fp@1877: {
fp@1877:     const ec_code_msg_t *error_msg;
fp@1877: 
fp@1877:     for (error_msg = soe_error_codes; error_msg->code; error_msg++) {
fp@1877:         if (error_msg->code == error_code) {
fp@1921:             EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n",
fp@1877:                    error_msg->code, error_msg->message);
fp@1877:             return;
fp@1877:         }
fp@1877:     }
fp@1877: 
fp@1921:     EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code);
fp@1877: }
fp@1877: 
fp@1877: /*****************************************************************************/
fp@1877: 
fp@1831: /** Constructor.
fp@1831:  */
fp@1831: void ec_fsm_soe_init(
fp@2589:         ec_fsm_soe_t *fsm /**< finite state machine */
fp@1873:         )
fp@1831: {
fp@1831:     fsm->state = NULL;
fp@2589:     fsm->datagram = NULL;
fp@2589:     fsm->fragment_size = 0;
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** Destructor.
fp@1831:  */
fp@1831: void ec_fsm_soe_clear(
fp@1873:         ec_fsm_soe_t *fsm /**< finite state machine */
fp@1873:         )
fp@1831: {
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** Starts to transfer an IDN to/from a slave.
fp@1831:  */
fp@1831: void ec_fsm_soe_transfer(
fp@1831:         ec_fsm_soe_t *fsm, /**< State machine. */
fp@1831:         ec_slave_t *slave, /**< EtherCAT slave. */
fp@1831:         ec_soe_request_t *request /**< SoE request. */
fp@1831:         )
fp@1831: {
fp@1831:     fsm->slave = slave;
fp@1831:     fsm->request = request;
fp@2589: 
fp@1831:     if (request->dir == EC_DIR_OUTPUT) {
fp@1837:         fsm->state = ec_fsm_soe_write_start;
fp@1873:     } else {
fp@1831:         fsm->state = ec_fsm_soe_read_start;
fp@1873:     }
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@2589: /** Executes the current state of the state machine.
fp@2589:  *
fp@2589:  * \return 1 if the datagram was used, else 0.
fp@2589:  */
fp@2589: int ec_fsm_soe_exec(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@2589:     int datagram_used = 0;
fp@2589: 
fp@2589:     if (fsm->datagram &&
fp@2589:             (fsm->datagram->state == EC_DATAGRAM_INIT ||
fp@2589:              fsm->datagram->state == EC_DATAGRAM_QUEUED ||
fp@2589:              fsm->datagram->state == EC_DATAGRAM_SENT)) {
fp@2589:         // datagram not received yet
fp@2589:         return datagram_used;
fp@2589:     }
fp@2589: 
fp@2589:     fsm->state(fsm, datagram);
fp@2589: 
fp@2589:     datagram_used =
fp@2589:         fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
fp@2589: 
fp@2589:     if (datagram_used) {
fp@2589:         fsm->datagram = datagram;
fp@2589:     } else {
fp@2589:         fsm->datagram = NULL;
fp@2589:     }
fp@2589: 
fp@2589:     return datagram_used;
fp@2589: }
fp@2589: 
fp@2589: /*****************************************************************************/
fp@2589: 
fp@2589: /** Returns, if the state machine terminated with success.
fp@2589:  *
fp@2589:  * \return non-zero if successful.
fp@2589:  */
fp@2589: int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
fp@1831: {
fp@1831:     return fsm->state == ec_fsm_soe_end;
fp@1831: }
fp@1831: 
fp@1881: /*****************************************************************************/
fp@1881: 
fp@1881: /** Output information about a failed SoE transfer.
fp@1881:  */
fp@1881: void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
fp@1881: {
fp@1881:     ec_soe_request_t *request = fsm->request;
fp@1881: 
fp@1921:     EC_SLAVE_ERR(fsm->slave, "");
fp@1881: 
fp@1881:     if (request->dir == EC_DIR_OUTPUT) {
fp@1881:         printk("Writing");
fp@1881:     } else {
fp@1881:         printk("Reading");
fp@1881:     }
fp@1881: 
fp@1921:     printk(" IDN 0x%04X failed.\n", request->idn);
fp@1881: }
fp@1881: 
fp@1831: /******************************************************************************
fp@1831:  * SoE read state machine
fp@1831:  *****************************************************************************/
fp@1831: 
fp@2589: /** Prepare a read operation.
fp@2589:  *
fp@2589:  * \return 0 on success, otherwise a negative error code.
fp@2589:  */
fp@2589: int ec_fsm_soe_prepare_read(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@2589:     uint8_t *data;
fp@1831:     ec_slave_t *slave = fsm->slave;
fp@1831:     ec_master_t *master = slave->master;
fp@1831:     ec_soe_request_t *request = fsm->request;
fp@2589: 
fp@2589:     data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
fp@1905:             EC_SOE_SIZE);
fp@1831:     if (IS_ERR(data)) {
fp@2589:         return PTR_ERR(data);
fp@1831:     }
fp@1831: 
fp@1952:     EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
fp@1831:     EC_WRITE_U8(data + 1, 1 << 6); // request value
fp@1831:     EC_WRITE_U16(data + 2, request->idn);
fp@1831: 
fp@1831:     if (master->debug_level) {
fp@1921:         EC_SLAVE_DBG(slave, 0, "SCC read request:\n");
fp@1905:         ec_print_data(data, EC_SOE_SIZE);
fp@1831:     }
fp@1831: 
fp@1831:     fsm->request->jiffies_sent = jiffies;
fp@2589:     fsm->state = ec_fsm_soe_read_request;
fp@2589: 
fp@2589:     return 0;
fp@2589: }
fp@2589: 
fp@2589: /*****************************************************************************/
fp@2589: 
fp@2589: /** SoE state: READ START.
fp@2589:  */
fp@2589: void ec_fsm_soe_read_start(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@2589:     ec_slave_t *slave = fsm->slave;
fp@2589:     ec_soe_request_t *request = fsm->request;
fp@2589: 
fp@2589:     EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
fp@2589:             request->drive_no);
fp@2589: 
fp@2589:     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
fp@2589:         EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
fp@2589:         fsm->state = ec_fsm_soe_error;
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     request->data_size = 0;
fp@1831:     fsm->retries = EC_FSM_RETRIES;
fp@2589: 
fp@2589:     if (ec_fsm_soe_prepare_read(fsm, datagram)) {
fp@2589:         fsm->state = ec_fsm_soe_error;
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:     }
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** SoE state: READ REQUEST.
fp@1831:  */
fp@2589: void ec_fsm_soe_read_request(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@1831:     ec_slave_t *slave = fsm->slave;
fp@1831:     unsigned long diff_ms;
fp@1831: 
fp@2589:     if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
fp@2589:         if (ec_fsm_soe_prepare_read(fsm, datagram)) {
fp@2589:             fsm->state = ec_fsm_soe_error;
fp@2589:             ec_fsm_soe_print_error(fsm);
fp@2589:         }
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
fp@2589:         ec_datagram_print_state(fsm->datagram);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1831:         return;
fp@1831:     }
fp@1831: 
fp@1831:     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
fp@1831: 
fp@2589:     if (fsm->datagram->working_counter != 1) {
fp@2589:         if (!fsm->datagram->working_counter) {
fp@1831:             if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
fp@1831:                 // no response; send request datagram again
fp@2589:                 if (ec_fsm_soe_prepare_read(fsm, datagram)) {
fp@2589:                     fsm->state = ec_fsm_soe_error;
fp@2589:                     ec_fsm_soe_print_error(fsm);
fp@2589:                 }
fp@1831:                 return;
fp@1831:             }
fp@1831:         }
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Reception of SoE read request"
fp@1931:                 " failed after %lu ms: ", diff_ms);
fp@2589:         ec_datagram_print_wc_error(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     fsm->jiffies_start = fsm->datagram->jiffies_sent;
fp@2589:     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1831:     fsm->retries = EC_FSM_RETRIES;
fp@1831:     fsm->state = ec_fsm_soe_read_check;
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** CoE state: READ CHECK.
fp@1831:  */
fp@2589: void ec_fsm_soe_read_check(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@2589:     ec_slave_t *slave = fsm->slave;
fp@2589: 
fp@2589:     if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
fp@2589:         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
fp@2589:         ec_datagram_print_state(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->working_counter != 1) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
fp@1921:                 " datagram failed: ");
fp@2589:         ec_datagram_print_wc_error(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (!ec_slave_mbox_check(fsm->datagram)) {
fp@1831:         unsigned long diff_ms =
fp@2589:             (fsm->datagram->jiffies_received - fsm->jiffies_start) *
fp@2589:             1000 / HZ;
fp@1831:         if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fp@1831:             fsm->state = ec_fsm_soe_error;
fp@1931:             EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
fp@1931:                     " read response.\n", diff_ms);
fp@1881:             ec_fsm_soe_print_error(fsm);
fp@1831:             return;
fp@1831:         }
fp@1831: 
fp@2589:         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1831:         fsm->retries = EC_FSM_RETRIES;
fp@1831:         return;
fp@1831:     }
fp@1831: 
fp@1831:     // Fetch response
fp@2589:     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fp@1831:     fsm->retries = EC_FSM_RETRIES;
fp@1831:     fsm->state = ec_fsm_soe_read_response;
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** SoE state: READ RESPONSE.
fp@1831:  */
fp@2589: void ec_fsm_soe_read_response(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@1831:     ec_slave_t *slave = fsm->slave;
fp@1831:     ec_master_t *master = slave->master;
fp@1864:     uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
fp@1864:             value_included;
fp@1831:     size_t rec_size, data_size;
fp@1831:     ec_soe_request_t *req = fsm->request;
fp@1831: 
fp@2589:     if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
fp@2589:         ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
fp@2589:         ec_datagram_print_state(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->working_counter != 1) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
fp@2589:         ec_datagram_print_wc_error(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
fp@1831:     if (IS_ERR(data)) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1831:         return;
fp@1831:     }
fp@1831: 
fp@1831:     if (master->debug_level) {
fp@1921:         EC_SLAVE_DBG(slave, 0, "SCC read response:\n");
fp@1831:         ec_print_data(data, rec_size);
fp@1831:     }
fp@1831: 
fp@1831:     if (mbox_prot != EC_MBOX_TYPE_SOE) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
fp@1921:                 mbox_prot);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1831:         return;
fp@1831:     }
fp@1831: 
fp@1905:     if (rec_size < EC_SOE_SIZE) {
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Received currupted SoE read response"
fp@1881:                 " (%zu bytes)!\n", rec_size);
fp@1831:         ec_print_data(data, rec_size);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1831:         return;
fp@1831:     }
fp@1831: 
fp@1864:     header = EC_READ_U8(data);
fp@1873:     opcode = header & 0x7;
fp@1864:     incomplete = (header >> 3) & 1;
fp@1873:     error_flag = (header >> 4) & 1;
fp@1864: 
fp@1905:     if (opcode != OPCODE_READ_RESPONSE) {
fp@1921:         EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n",
fp@1921:                 opcode);
fp@1831:         ec_print_data(data, rec_size);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1831:         fsm->state = ec_fsm_soe_error;
fp@1831:         return;
fp@1831:     }
fp@1831: 
fp@1873:     if (error_flag) {
fp@1873:         req->error_code = EC_READ_U16(data + rec_size - 2);
fp@1921:         EC_SLAVE_ERR(slave, "Received error response:\n");
fp@1921:         ec_print_soe_error(slave, req->error_code);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1873:         fsm->state = ec_fsm_soe_error;
fp@1873:         return;
fp@1873:     } else {
fp@1873:         req->error_code = 0x0000;
fp@1873:     }
fp@1873: 
fp@1873:     value_included = (EC_READ_U8(data + 1) >> 6) & 1;
fp@1873:     if (!value_included) {
fp@1921:         EC_SLAVE_ERR(slave, "No value included!\n");
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1873:         fsm->state = ec_fsm_soe_error;
fp@1873:         return;
fp@1873:     }
fp@1873: 
fp@1905:     data_size = rec_size - EC_SOE_SIZE;
fp@1873:     if (ec_soe_request_append_data(req,
fp@1905:                 data + EC_SOE_SIZE, data_size)) {
fp@1873:         fsm->state = ec_fsm_soe_error;
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1873:         return;
fp@1873:     }
fp@1831: 
fp@1864:     if (incomplete) {
fp@1921:         EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
fp@1921:                 " at offset %zu.\n", req->data_size);
fp@2589:         fsm->jiffies_start = fsm->datagram->jiffies_sent;
fp@2589:         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1864:         fsm->retries = EC_FSM_RETRIES;
fp@1864:         fsm->state = ec_fsm_soe_read_check;
fp@1864:     } else {
fp@1864:         if (master->debug_level) {
fp@1921:             EC_SLAVE_DBG(slave, 0, "IDN data:\n");
fp@1864:             ec_print_data(req->data, req->data_size);
fp@1864:         }
fp@1864: 
fp@1864:         fsm->state = ec_fsm_soe_end; // success
fp@1864:     }
fp@1831: }
fp@1831: 
fp@1837: /******************************************************************************
fp@1837:  * SoE write state machine
fp@1837:  *****************************************************************************/
fp@1837: 
fp@1865: /** Write next fragment.
fp@1865:  */
fp@1865: void ec_fsm_soe_write_next_fragment(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@1865:     ec_slave_t *slave = fsm->slave;
fp@1865:     ec_master_t *master = slave->master;
fp@1865:     ec_soe_request_t *req = fsm->request;
fp@1865:     uint8_t incomplete, *data;
fp@2589:     size_t header_size, max_fragment_size, remaining_size;
fp@1865:     uint16_t fragments_left;
fp@1865: 
fp@1905:     header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
fp@1865:     if (slave->configured_rx_mailbox_size <= header_size) {
fp@1921:         EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
fp@1865:                 slave->configured_rx_mailbox_size);
fp@1865:         fsm->state = ec_fsm_soe_error;
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1865:         return;
fp@1865:     }
fp@1865: 
fp@1865:     remaining_size = req->data_size - fsm->offset;
fp@1865:     max_fragment_size = slave->configured_rx_mailbox_size - header_size;
fp@1865:     incomplete = remaining_size > max_fragment_size;
fp@2589:     fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
fp@2589:     fragments_left = remaining_size / fsm->fragment_size - 1;
fp@2589:     if (remaining_size % fsm->fragment_size) {
fp@1865:         fragments_left++;
fp@1865:     }
fp@1865: 
fp@2589:     data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
fp@2589:             EC_SOE_SIZE + fsm->fragment_size);
fp@1865:     if (IS_ERR(data)) {
fp@1865:         fsm->state = ec_fsm_soe_error;
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1865:         return;
fp@1865:     }
fp@1865: 
fp@1952:     EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |
fp@1952:             (req->drive_no & 0x07) << 5);
fp@1865:     EC_WRITE_U8(data + 1, 1 << 6); // only value included
fp@1866:     EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
fp@2589:     memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size);
fp@1865: 
fp@1865:     if (master->debug_level) {
fp@1921:         EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
fp@2589:         ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
fp@1865:     }
fp@1865: 
fp@1865:     req->jiffies_sent = jiffies;
fp@1865:     fsm->state = ec_fsm_soe_write_request;
fp@1865: }
fp@1865: 
fp@1865: /*****************************************************************************/
fp@1865: 
fp@1837: /** SoE state: WRITE START.
fp@1837:  */
fp@2589: void ec_fsm_soe_write_start(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@1837: {
fp@1837:     ec_slave_t *slave = fsm->slave;
fp@1865:     ec_soe_request_t *req = fsm->request;
fp@1837: 
fp@1952:     EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n",
fp@1952:             req->idn, req->drive_no, req->data_size);
fp@1837: 
fp@1837:     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
fp@1921:         EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
fp@1881:         fsm->state = ec_fsm_soe_error;
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1837:         return;
fp@1837:     }
fp@1837: 
fp@1865:     fsm->offset = 0;
fp@2589:     fsm->retries = EC_FSM_RETRIES;
fp@2589:     ec_fsm_soe_write_next_fragment(fsm, datagram);
fp@1837: }
fp@1837: 
fp@1837: /*****************************************************************************/
fp@1837: 
fp@1837: /** SoE state: WRITE REQUEST.
fp@1837:  */
fp@2589: void ec_fsm_soe_write_request(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@1837:     ec_slave_t *slave = fsm->slave;
fp@1837:     unsigned long diff_ms;
fp@1837: 
fp@2589:     if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
fp@2589:         ec_fsm_soe_write_next_fragment(fsm, datagram);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
fp@2589:         ec_datagram_print_state(fsm->datagram);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1837:         return;
fp@1837:     }
fp@1837: 
fp@1837:     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
fp@1837: 
fp@2589:     if (fsm->datagram->working_counter != 1) {
fp@2589:         if (!fsm->datagram->working_counter) {
fp@1837:             if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
fp@1837:                 // no response; send request datagram again
fp@2589:                 ec_fsm_soe_write_next_fragment(fsm, datagram);
fp@1837:                 return;
fp@1837:             }
fp@1837:         }
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Reception of SoE write request"
fp@1931:                 " failed after %lu ms: ", diff_ms);
fp@2589:         ec_datagram_print_wc_error(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     fsm->jiffies_start = fsm->datagram->jiffies_sent;
fp@2589: 
fp@2589:     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1837:     fsm->retries = EC_FSM_RETRIES;
fp@1837:     fsm->state = ec_fsm_soe_write_check;
fp@1837: }
fp@1837: 
fp@1837: /*****************************************************************************/
fp@1837: 
fp@1837: /** CoE state: WRITE CHECK.
fp@1837:  */
fp@2589: void ec_fsm_soe_write_check(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@2589:     ec_slave_t *slave = fsm->slave;
fp@2589: 
fp@2589:     if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
fp@2589:         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
fp@2589:         ec_datagram_print_state(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->working_counter != 1) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
fp@2589:         ec_datagram_print_wc_error(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (!ec_slave_mbox_check(fsm->datagram)) {
fp@2589:         unsigned long diff_ms =
fp@2589:             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
fp@2589:         if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fp@2589:             fsm->state = ec_fsm_soe_error;
fp@2589:             EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
fp@2589:                     " for write response.\n", diff_ms);
fp@2589:             ec_fsm_soe_print_error(fsm);
fp@1837:             return;
fp@1837:         }
fp@1837: 
fp@2589:         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fp@1837:         fsm->retries = EC_FSM_RETRIES;
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     // Fetch response
fp@2589:     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fp@2589:     fsm->retries = EC_FSM_RETRIES;
fp@2589:     fsm->state = ec_fsm_soe_write_response;
fp@1837: }
fp@1837: 
fp@1837: /*****************************************************************************/
fp@1837: 
fp@1837: /** SoE state: WRITE RESPONSE.
fp@1837:  */
fp@2589: void ec_fsm_soe_write_response(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@1837:     ec_slave_t *slave = fsm->slave;
fp@1837:     ec_master_t *master = slave->master;
fp@1866:     ec_soe_request_t *req = fsm->request;
fp@1837:     uint8_t *data, mbox_prot, opcode, error_flag;
fp@1873:     uint16_t idn;
fp@1837:     size_t rec_size;
fp@1837: 
fp@2589:     if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
fp@2589:         ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fp@1837:         return; // FIXME: request again?
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Failed to receive SoE write"
fp@1921:                 " response datagram: ");
fp@2589:         ec_datagram_print_state(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     if (fsm->datagram->working_counter != 1) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
fp@2589:         ec_datagram_print_wc_error(fsm->datagram);
fp@2589:         ec_fsm_soe_print_error(fsm);
fp@2589:         return;
fp@2589:     }
fp@2589: 
fp@2589:     data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
fp@1837:     if (IS_ERR(data)) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1837:         return;
fp@1837:     }
fp@1837: 
fp@1837:     if (master->debug_level) {
fp@1921:         EC_SLAVE_DBG(slave, 0, "SCC write response:\n");
fp@1837:         ec_print_data(data, rec_size);
fp@1837:     }
fp@1837: 
fp@1837:     if (mbox_prot != EC_MBOX_TYPE_SOE) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1921:         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
fp@1921:                 mbox_prot);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1837:         return;
fp@1837:     }
fp@1837: 
fp@1905:     if (rec_size < EC_SOE_SIZE) {
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@2589:         EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
fp@1921:                 " (%zu bytes)!\n", rec_size);
fp@1873:         ec_print_data(data, rec_size);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1873:         return;
fp@1873:     }
fp@1873: 
fp@1873:     opcode = EC_READ_U8(data) & 0x7;
fp@1905:     if (opcode != OPCODE_WRITE_RESPONSE) {
fp@1921:         EC_SLAVE_ERR(slave, "Received no write response"
fp@1921:                 " (opcode %x).\n", opcode);
fp@1837:         ec_print_data(data, rec_size);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1837:         fsm->state = ec_fsm_soe_error;
fp@1837:         return;
fp@1837:     }
fp@1837: 
fp@1873:     idn = EC_READ_U16(data + 2);
fp@1873:     if (idn != req->idn) {
fp@1921:         EC_SLAVE_ERR(slave, "Received response for"
fp@1921:                 " wrong IDN 0x%04x.\n", idn);
fp@1873:         ec_print_data(data, rec_size);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1873:         fsm->state = ec_fsm_soe_error;
fp@1873:         return;
fp@1873:     }
fp@1873: 
fp@1873:     error_flag = (EC_READ_U8(data) >> 4) & 1;
fp@1873:     if (error_flag) {
fp@1905:         if (rec_size < EC_SOE_SIZE + 2) {
fp@1921:             EC_SLAVE_ERR(slave, "Received corrupted error response"
fp@1921:                     " - error flag set, but received size is %zu.\n",
fp@1921:                     rec_size);
fp@1873:         } else {
fp@1905:             req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
fp@1921:             EC_SLAVE_ERR(slave, "Received error response:\n");
fp@1921:             ec_print_soe_error(slave, req->error_code);
fp@1873:         }
fp@1873:         ec_print_data(data, rec_size);
fp@1881:         ec_fsm_soe_print_error(fsm);
fp@1873:         fsm->state = ec_fsm_soe_error;
fp@1873:         return;
fp@1873:     } else {
fp@1873:         req->error_code = 0x0000;
fp@1873:     }
fp@1837: 
fp@2589:     fsm->offset += fsm->fragment_size;
fp@2589: 
fp@2589:     if (fsm->offset < req->data_size) {
fp@2589:         fsm->retries = EC_FSM_RETRIES;
fp@2589:         ec_fsm_soe_write_next_fragment(fsm, datagram);
fp@2589:     } else {
fp@2589:         fsm->state = ec_fsm_soe_end; // success
fp@2589:     }
fp@1837: }
fp@1837: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** State: ERROR.
fp@1831:  */
fp@2589: void ec_fsm_soe_error(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@1831: {
fp@1831: }
fp@1831: 
fp@1831: /*****************************************************************************/
fp@1831: 
fp@1831: /** State: END.
fp@1831:  */
fp@2589: void ec_fsm_soe_end(
fp@2589:         ec_fsm_soe_t *fsm, /**< finite state machine */
fp@2589:         ec_datagram_t *datagram /**< Datagram to use. */
fp@2589:         )
fp@2589: {
fp@2589: }
fp@2589: 
fp@2589: /*****************************************************************************/