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@1831: void ec_fsm_soe_read_start(ec_fsm_soe_t *); fp@1831: void ec_fsm_soe_read_request(ec_fsm_soe_t *); fp@1831: void ec_fsm_soe_read_check(ec_fsm_soe_t *); fp@1831: void ec_fsm_soe_read_response(ec_fsm_soe_t *); fp@1831: fp@1837: void ec_fsm_soe_write_start(ec_fsm_soe_t *); fp@1837: void ec_fsm_soe_write_request(ec_fsm_soe_t *); fp@1837: void ec_fsm_soe_write_check(ec_fsm_soe_t *); fp@1837: void ec_fsm_soe_write_response(ec_fsm_soe_t *); fp@1837: fp@1831: void ec_fsm_soe_end(ec_fsm_soe_t *); fp@1831: void ec_fsm_soe_error(ec_fsm_soe_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@1873: ec_fsm_soe_t *fsm, /**< finite state machine */ fp@1873: ec_datagram_t *datagram /**< datagram */ fp@1873: ) fp@1831: { fp@1831: fsm->state = NULL; fp@1831: fsm->datagram = datagram; 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@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@1831: /** fp@1831: Executes the current state of the state machine. fp@1831: \return false, if state machine has terminated fp@1831: */ fp@1831: fp@1831: int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: fsm->state(fsm); fp@1831: fp@1831: return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error; fp@1831: } fp@1831: fp@1831: /*****************************************************************************/ fp@1831: fp@1831: /** fp@1831: Returns, if the state machine terminated with success. fp@1831: \return non-zero if successful. fp@1831: */ fp@1831: fp@1831: int ec_fsm_soe_success(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@1831: /** SoE state: READ START. fp@1831: */ fp@1831: void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: ec_datagram_t *datagram = fsm->datagram; 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@1831: uint8_t *data; fp@1831: fp@1952: EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn, fp@1952: request->drive_no); fp@1831: fp@1831: 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@1831: return; fp@1831: } fp@1831: fp@1831: data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, fp@1905: EC_SOE_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@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@1865: fsm->request->data_size = 0; fp@1831: fsm->request->jiffies_sent = jiffies; fp@1831: fsm->retries = EC_FSM_RETRIES; fp@1831: fsm->state = ec_fsm_soe_read_request; fp@1831: } fp@1831: fp@1831: /*****************************************************************************/ fp@1831: fp@1831: /** SoE state: READ REQUEST. fp@1831: */ fp@1831: void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: ec_datagram_t *datagram = fsm->datagram; fp@1831: ec_slave_t *slave = fsm->slave; fp@1831: unsigned long diff_ms; fp@1831: fp@1831: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1831: return; // FIXME: check for response first? fp@1831: fp@1831: if (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@1831: ec_datagram_print_state(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@1831: if (datagram->working_counter != 1) { fp@1831: if (!datagram->working_counter) { fp@1831: if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { fp@1831: // no response; send request datagram again 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@1831: ec_datagram_print_wc_error(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1831: return; fp@1831: } fp@1831: fp@1831: fsm->jiffies_start = datagram->jiffies_sent; fp@1831: 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@1831: void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: ec_datagram_t *datagram = fsm->datagram; fp@1831: ec_slave_t *slave = fsm->slave; fp@1831: fp@1831: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1831: return; fp@1831: fp@1831: if (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@1831: ec_datagram_print_state(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1831: return; fp@1831: } fp@1831: fp@1831: if (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@1831: ec_datagram_print_wc_error(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1831: return; fp@1831: } fp@1831: fp@1831: if (!ec_slave_mbox_check(datagram)) { fp@1831: unsigned long diff_ms = fp@1831: (datagram->jiffies_received - fsm->jiffies_start) * 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@1831: 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@1831: 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@1831: void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: ec_datagram_t *datagram = fsm->datagram; 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@1831: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1831: return; // FIXME: request again? fp@1831: fp@1831: if (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@1831: ec_datagram_print_state(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1831: return; fp@1831: } fp@1831: fp@1831: if (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@1831: ec_datagram_print_wc_error(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1831: return; fp@1831: } fp@1831: fp@1831: data = ec_slave_mbox_fetch(slave, 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@1864: fsm->jiffies_start = datagram->jiffies_sent; fp@1864: 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@1865: ec_fsm_soe_t *fsm /**< finite state machine */ fp@1865: ) fp@1865: { fp@1865: ec_datagram_t *datagram = fsm->datagram; 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@1865: size_t header_size, max_fragment_size, remaining_size, fragment_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@1865: fragment_size = incomplete ? max_fragment_size : remaining_size; fp@1866: fragments_left = remaining_size / fragment_size - 1; fp@1865: if (remaining_size % fragment_size) { fp@1865: fragments_left++; fp@1865: } fp@1865: fp@1865: data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, fp@1905: EC_SOE_SIZE + 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@1873: memcpy(data + 4, req->data + fsm->offset, fragment_size); fp@1865: fsm->offset += fragment_size; fp@1865: fp@1865: if (master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 0, "SCC write request:\n"); fp@1905: ec_print_data(data, EC_SOE_SIZE + fragment_size); fp@1865: } fp@1865: fp@1865: req->jiffies_sent = jiffies; fp@1865: fsm->retries = EC_FSM_RETRIES; 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@1837: void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) 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@1865: ec_fsm_soe_write_next_fragment(fsm); fp@1837: } fp@1837: fp@1837: /*****************************************************************************/ fp@1837: fp@1837: /** SoE state: WRITE REQUEST. fp@1837: */ fp@1837: void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1837: { fp@1837: ec_datagram_t *datagram = fsm->datagram; fp@1837: ec_slave_t *slave = fsm->slave; fp@1837: unsigned long diff_ms; fp@1837: fp@1837: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1837: return; // FIXME: check for response first? fp@1837: fp@1837: if (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@1837: ec_datagram_print_state(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@1837: if (datagram->working_counter != 1) { fp@1837: if (!datagram->working_counter) { fp@1837: if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { fp@1837: // no response; send request datagram again 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@1837: ec_datagram_print_wc_error(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1837: return; fp@1837: } fp@1837: fp@1837: fsm->jiffies_start = datagram->jiffies_sent; fp@1837: fp@1837: 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@1837: void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1837: { fp@1837: ec_datagram_t *datagram = fsm->datagram; fp@1837: ec_slave_t *slave = fsm->slave; fp@1866: ec_soe_request_t *req = fsm->request; fp@1837: fp@1837: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1837: return; fp@1837: fp@1837: if (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@1837: ec_datagram_print_state(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1837: return; fp@1837: } fp@1837: fp@1837: if (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@1837: ec_datagram_print_wc_error(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1837: return; fp@1837: } fp@1837: fp@1866: if (fsm->offset < req->data_size) { fp@1866: ec_fsm_soe_write_next_fragment(fsm); fp@1866: } else { fp@1866: if (!ec_slave_mbox_check(datagram)) { fp@1866: unsigned long diff_ms = fp@1866: (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; fp@1866: if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { fp@1866: fsm->state = ec_fsm_soe_error; fp@1931: EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" fp@1931: " for write response.\n", diff_ms); fp@1881: ec_fsm_soe_print_error(fsm); fp@1866: return; fp@1866: } fp@1866: fp@1866: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1866: fsm->retries = EC_FSM_RETRIES; fp@1837: return; fp@1837: } fp@1837: fp@1866: // Fetch response fp@1866: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@1837: fsm->retries = EC_FSM_RETRIES; fp@1866: fsm->state = ec_fsm_soe_write_response; fp@1866: } fp@1837: } fp@1837: fp@1837: /*****************************************************************************/ fp@1837: fp@1837: /** SoE state: WRITE RESPONSE. fp@1837: */ fp@1837: void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1837: { fp@1837: ec_datagram_t *datagram = fsm->datagram; 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@1837: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1837: return; // FIXME: request again? fp@1837: fp@1837: if (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@1837: ec_datagram_print_state(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1837: return; fp@1837: } fp@1837: fp@1837: if (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@1837: ec_datagram_print_wc_error(datagram); fp@1881: ec_fsm_soe_print_error(fsm); fp@1837: return; fp@1837: } fp@1837: fp@1837: data = ec_slave_mbox_fetch(slave, 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@1921: EC_SLAVE_ERR(slave, "Received currupted 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@1866: fsm->state = ec_fsm_soe_end; // success fp@1837: } fp@1837: fp@1831: /*****************************************************************************/ fp@1831: fp@1831: /** State: ERROR. fp@1831: */ fp@1831: void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: } fp@1831: fp@1831: /*****************************************************************************/ fp@1831: fp@1831: /** State: END. fp@1831: */ fp@1831: void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */) fp@1831: { fp@1831: } fp@1831: fp@1831: /*****************************************************************************/