fp@436: /****************************************************************************** fp@436: * fp@436: * $Id$ fp@436: * fp@1326: * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH fp@436: * fp@436: * This file is part of the IgH EtherCAT Master. fp@436: * fp@1326: * The IgH EtherCAT Master is free software; you can redistribute it and/or fp@1326: * modify it under the terms of the GNU General Public License version 2, as fp@1326: * published by the Free Software Foundation. fp@436: * fp@1326: * The IgH EtherCAT Master is distributed in the hope that it will be useful, fp@1326: * but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1326: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General fp@1326: * Public License for more details. fp@436: * fp@1326: * You should have received a copy of the GNU General Public License along fp@1326: * with the IgH EtherCAT Master; if not, write to the Free Software fp@436: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@436: * fp@1363: * --- fp@1363: * fp@1363: * The license mentioned above concerns the source code only. Using the fp@1363: * EtherCAT technology and brand is only permitted in compliance with the fp@1363: * industrial property and similar rights of Beckhoff Automation GmbH. fp@436: * fp@436: *****************************************************************************/ fp@436: fp@2589: /** \file fp@2589: * EtherCAT CoE state machines. fp@2589: */ fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: #include "globals.h" fp@436: #include "master.h" fp@436: #include "mailbox.h" fp@436: #include "fsm_coe.h" fp@2589: #include "slave_config.h" fp@436: fp@436: /*****************************************************************************/ fp@436: fp@1053: /** Maximum time in ms to wait for responses when reading out the dictionary. fp@1053: */ fp@1794: #define EC_FSM_COE_DICT_TIMEOUT 1000 fp@1053: fp@1907: /** CoE download request header size. fp@1907: */ fp@1907: #define EC_COE_DOWN_REQ_HEADER_SIZE 10 fp@1907: fp@1907: /** CoE download segment request header size. fp@1907: */ fp@1907: #define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3 fp@1907: fp@1907: /** Minimum size of download segment. fp@1907: */ fp@1907: #define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7 fp@1907: fp@1907: /** Enable debug output for CoE retries. fp@1907: */ fp@1791: #define DEBUG_RETRIES 0 fp@1907: fp@1907: /** Enable warning output if transfers take too long. fp@1907: */ fp@1792: #define DEBUG_LONG 0 fp@1791: fp@1053: /*****************************************************************************/ fp@1053: fp@2589: void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: fp@2589: void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: fp@2589: void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: fp@2589: void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *); fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@2589: /** SDO abort messages. fp@2589: * fp@2589: * The "abort SDO transfer request" supplies an abort code, which can be fp@2589: * translated to clear text. This table does the mapping of the codes and fp@2589: * messages. fp@2589: */ fp@436: const ec_code_msg_t sdo_abort_messages[] = { fp@436: {0x05030000, "Toggle bit not changed"}, fp@1327: {0x05040000, "SDO protocol timeout"}, fp@436: {0x05040001, "Client/Server command specifier not valid or unknown"}, fp@436: {0x05040005, "Out of memory"}, fp@436: {0x06010000, "Unsupported access to an object"}, fp@436: {0x06010001, "Attempt to read a write-only object"}, fp@436: {0x06010002, "Attempt to write a read-only object"}, fp@436: {0x06020000, "This object does not exist in the object directory"}, fp@1327: {0x06040041, "The object cannot be mapped into the PDO"}, fp@436: {0x06040042, "The number and length of the objects to be mapped would" fp@1327: " exceed the PDO length"}, fp@436: {0x06040043, "General parameter incompatibility reason"}, fp@436: {0x06040047, "Gerneral internal incompatibility in device"}, fp@436: {0x06060000, "Access failure due to a hardware error"}, fp@436: {0x06070010, "Data type does not match, length of service parameter does" fp@436: " not match"}, fp@436: {0x06070012, "Data type does not match, length of service parameter too" fp@436: " high"}, fp@436: {0x06070013, "Data type does not match, length of service parameter too" fp@436: " low"}, fp@436: {0x06090011, "Subindex does not exist"}, fp@436: {0x06090030, "Value range of parameter exceeded"}, fp@436: {0x06090031, "Value of parameter written too high"}, fp@436: {0x06090032, "Value of parameter written too low"}, fp@436: {0x06090036, "Maximum value is less than minimum value"}, fp@436: {0x08000000, "General error"}, fp@436: {0x08000020, "Data cannot be transferred or stored to the application"}, fp@436: {0x08000021, "Data cannot be transferred or stored to the application" fp@436: " because of local control"}, fp@436: {0x08000022, "Data cannot be transferred or stored to the application" fp@436: " because of the present device state"}, fp@436: {0x08000023, "Object dictionary dynamic generation fails or no object" fp@436: " dictionary is present"}, fp@436: {} fp@436: }; fp@436: fp@436: /*****************************************************************************/ fp@436: fp@1921: /** Outputs an SDO abort message. fp@1921: */ fp@2589: void ec_canopen_abort_msg( fp@2589: const ec_slave_t *slave, /**< Slave. */ fp@2589: uint32_t abort_code /**< Abort code to search for. */ fp@2589: ) fp@436: { fp@436: const ec_code_msg_t *abort_msg; fp@436: fp@436: for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { fp@436: if (abort_msg->code == abort_code) { fp@1921: EC_SLAVE_ERR(slave, "SDO abort message 0x%08X: \"%s\".\n", fp@436: abort_msg->code, abort_msg->message); fp@436: return; fp@436: } fp@436: } fp@436: fp@1921: EC_SLAVE_ERR(slave, "Unknown SDO abort code 0x%08X.\n", abort_code); fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@2589: /** Constructor. fp@2589: */ fp@2589: void ec_fsm_coe_init( fp@2589: ec_fsm_coe_t *fsm /**< Finite state machine */ fp@2589: ) fp@436: { fp@436: fsm->state = NULL; fp@2589: fsm->datagram = NULL; fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@2589: /** Destructor. fp@2589: */ fp@2589: void ec_fsm_coe_clear( fp@2589: ec_fsm_coe_t *fsm /**< Finite state machine */ fp@2589: ) fp@2589: { fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@2589: /** Starts reading a slaves' SDO dictionary. fp@2589: */ fp@2589: void ec_fsm_coe_dictionary( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine */ fp@2589: ec_slave_t *slave /**< EtherCAT slave */ fp@2589: ) fp@436: { fp@436: fsm->slave = slave; fp@436: fsm->state = ec_fsm_coe_dict_start; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@2589: /** Starts to transfer an SDO to/from a slave. fp@2589: */ fp@859: void ec_fsm_coe_transfer( fp@854: ec_fsm_coe_t *fsm, /**< State machine. */ fp@854: ec_slave_t *slave, /**< EtherCAT slave. */ fp@1327: ec_sdo_request_t *request /**< SDO request. */ fp@854: ) fp@436: { fp@436: fsm->slave = slave; fp@854: fsm->request = request; fp@2589: fp@2589: if (request->dir == EC_DIR_OUTPUT) { fp@859: fsm->state = ec_fsm_coe_down_start; fp@2589: } fp@2589: else { fp@859: fsm->state = ec_fsm_coe_up_start; fp@2589: } fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: 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_coe_exec( fp@2589: ec_fsm_coe_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_coe_end && fsm->state != ec_fsm_coe_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: * \return non-zero if successful. fp@2589: */ fp@2589: int ec_fsm_coe_success( fp@2589: const ec_fsm_coe_t *fsm /**< Finite state machine */ fp@2589: ) fp@436: { fp@436: return fsm->state == ec_fsm_coe_end; fp@436: } fp@436: fp@895: /*****************************************************************************/ fp@895: fp@1092: /** Check if the received data are a CoE emergency request. fp@1092: * fp@1092: * If the check is positive, the emergency request is output. fp@1092: * fp@1092: * \return The data were an emergency request. fp@895: */ fp@895: int ec_fsm_coe_check_emergency( fp@895: ec_fsm_coe_t *fsm, /**< Finite state machine */ fp@895: const uint8_t *data, /**< CoE mailbox data. */ fp@895: size_t size /**< CoE mailbox data size. */ fp@895: ) fp@895: { fp@895: if (size < 2 || ((EC_READ_U16(data) >> 12) & 0x0F) != 0x01) fp@895: return 0; fp@895: fp@895: if (size < 10) { fp@1921: EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency" fp@1921: " request:\n"); fp@895: ec_print_data(data, size); fp@895: return 1; fp@895: } fp@2589: fp@2589: { fp@2589: ec_slave_config_t *sc = fsm->slave->config; fp@2589: if (sc) { fp@2589: ec_coe_emerg_ring_push(&sc->emerg_ring, data + 2); fp@2589: } fp@2589: } fp@2589: fp@1921: EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n" fp@1921: "Error code 0x%04X, Error register 0x%02X, data:\n", fp@895: EC_READ_U16(data + 2), EC_READ_U8(data + 4)); fp@895: ec_print_data(data + 5, 5); fp@895: return 1; fp@895: } fp@895: fp@436: /****************************************************************************** fp@436: * CoE dictionary state machine fp@436: *****************************************************************************/ fp@436: fp@2589: /** Prepare a dictionary request. fp@2589: * fp@2589: * \return Zero on success, otherwise a negative error code. fp@2589: */ fp@2589: int ec_fsm_coe_prepare_dict( fp@2589: ec_fsm_coe_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: uint8_t *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); fp@1313: if (IS_ERR(data)) { fp@2589: return PTR_ERR(data); fp@436: } fp@436: fp@1327: EC_WRITE_U16(data, 0x8 << 12); // SDO information fp@436: EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request fp@436: EC_WRITE_U8 (data + 3, 0x00); fp@436: EC_WRITE_U16(data + 4, 0x0000); fp@1327: EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! fp@436: fp@2589: fsm->state = ec_fsm_coe_dict_request; fp@2589: return 0; fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@2589: /** CoE state: DICT START. fp@2589: */ fp@2589: void ec_fsm_coe_dict_start( fp@2589: ec_fsm_coe_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 (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { fp@2589: EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: return; fp@2589: } fp@2589: fp@2589: if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) { fp@2589: EC_SLAVE_ERR(slave, "Slave does not support" fp@2589: " SDO information service!\n"); fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: return; fp@2589: } fp@2589: fp@505: fsm->retries = EC_FSM_RETRIES; fp@2589: fp@2589: if (ec_fsm_coe_prepare_dict(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@2589: /** CoE state: DICT REQUEST. fp@2589: * \todo Timeout behavior fp@2589: */ fp@2589: void ec_fsm_coe_dict_request( fp@2589: ec_fsm_coe_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: if (ec_fsm_coe_prepare_dict(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" fp@1921: " request datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); 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@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_check; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@2589: /** CoE state: DICT CHECK. fp@2589: */ fp@2589: void ec_fsm_coe_dict_check( fp@2589: ec_fsm_coe_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@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" fp@1921: " datagram failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1038: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1053: if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fp@436: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Timeout while waiting for" fp@1921: " SDO dictionary list response.\n"); fp@436: return; fp@436: } fp@436: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: return; fp@436: } fp@436: fp@436: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_response; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@2589: /** Prepare an object description request. fp@2589: * fp@2589: * \return Zero on success, otherwise a negative error code. fp@2589: */ fp@2589: int ec_fsm_coe_dict_prepare_desc( fp@2589: ec_fsm_coe_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: u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); fp@2589: if (IS_ERR(data)) { fp@2589: return PTR_ERR(data); fp@2589: } fp@2589: fp@2589: EC_WRITE_U16(data, 0x8 << 12); // SDO information fp@2589: EC_WRITE_U8 (data + 2, 0x03); // Get object description request fp@2589: EC_WRITE_U8 (data + 3, 0x00); fp@2589: EC_WRITE_U16(data + 4, 0x0000); fp@2589: EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index fp@2589: fp@2589: fsm->state = ec_fsm_coe_dict_desc_request; fp@2589: return 0; fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@436: /** fp@436: CoE state: DICT RESPONSE. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@436: uint8_t *data, mbox_prot; fp@436: size_t rec_size; fp@436: unsigned int sdo_count, i; fp@769: uint16_t sdo_index, fragments_left; fp@436: ec_sdo_t *sdo; ch1010277@2045: bool first_segment; ch1010277@2045: size_t index_list_offset; ch1010277@2045: 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@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" fp@1921: " response datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1313: if (IS_ERR(data)) { fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: if (mbox_prot != 0x03) { // CoE fp@1921: EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", fp@1921: mbox_prot); fp@436: fsm->state = ec_fsm_coe_error; fp@769: return; fp@436: } fp@436: fp@895: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@895: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@895: fsm->retries = EC_FSM_RETRIES; fp@895: fsm->state = ec_fsm_coe_dict_check; fp@895: return; fp@895: } fp@895: fp@1037: if (rec_size < 3) { fp@1921: EC_SLAVE_ERR(slave, "Received corrupted SDO dictionary response" fp@1921: " (size %zu).\n", rec_size); fp@1037: fsm->state = ec_fsm_coe_error; fp@1037: return; fp@1037: } fp@1037: fp@1327: if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information fp@436: (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response fp@1921: EC_SLAVE_ERR(slave, "SDO information error response!\n"); fp@1037: if (rec_size < 10) { fp@1921: EC_SLAVE_ERR(slave, "Incomplete SDO information" fp@1921: " error response:\n"); fp@1037: ec_print_data(data, rec_size); fp@1037: } else { fp@1921: ec_canopen_abort_msg(slave, EC_READ_U32(data + 6)); fp@1037: } fp@436: fsm->state = ec_fsm_coe_error; fp@769: return; fp@436: } fp@436: fp@1327: if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information fp@436: (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response fp@1053: if (fsm->slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!" fp@1921: " Retrying...\n"); fp@1053: ec_print_data(data, rec_size); fp@1053: } fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1053: fsm->retries = EC_FSM_RETRIES; fp@1053: fsm->state = ec_fsm_coe_dict_check; fp@769: return; fp@436: } fp@436: ch1010277@2045: first_segment = list_empty(&slave->sdo_dictionary) ? true : false; ch1010277@2045: index_list_offset = first_segment ? 8 : 6; ch1010866@2041: fp@2194: if (rec_size < index_list_offset || rec_size % 2) { fp@2589: EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size); fp@2194: ec_print_data(data, rec_size); fp@2194: fsm->state = ec_fsm_coe_error; fp@2194: return; fp@2194: } fp@2194: ch1010866@2041: sdo_count = (rec_size - index_list_offset) / 2; fp@436: fp@436: for (i = 0; i < sdo_count; i++) { ch1010866@2041: sdo_index = EC_READ_U16(data + index_list_offset + i * 2); fp@436: if (!sdo_index) { fp@1921: EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n"); fp@436: continue; fp@436: } fp@436: fp@998: if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) { fp@1921: EC_SLAVE_ERR(slave, "Failed to allocate memory for SDO!\n"); fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@992: ec_sdo_init(sdo, slave, sdo_index); fp@436: list_add_tail(&sdo->list, &slave->sdo_dictionary); fp@436: } fp@436: fp@769: fragments_left = EC_READ_U16(data + 4); fp@1923: if (fragments_left) { fp@1921: EC_SLAVE_DBG(slave, 1, "SDO list fragments left: %u\n", fp@1921: fragments_left); fp@1921: } fp@1921: fp@1921: if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { fp@1921: // more messages waiting. check again. fp@2589: fsm->jiffies_start = fsm->datagram->jiffies_sent; fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_check; fp@436: return; fp@436: } fp@436: fp@436: if (list_empty(&slave->sdo_dictionary)) { fp@1327: // no SDOs in dictionary. finished. fp@436: fsm->state = ec_fsm_coe_end; // success fp@436: return; fp@436: } fp@436: fp@1327: // fetch SDO descriptions fp@436: fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); fp@436: fp@505: fsm->retries = EC_FSM_RETRIES; fp@2589: if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: DICT DESC REQUEST. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_desc_request( fp@2589: ec_fsm_coe_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: if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" fp@1921: " description request datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE SDO description" fp@1921: " request failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); 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@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_desc_check; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: DICT DESC CHECK. fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_desc_check( fp@2589: ec_fsm_coe_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@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" fp@1921: " datagram failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1038: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1053: if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fp@436: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Timeout while waiting for" fp@1921: " SDO 0x%04x object description response.\n", fp@1921: fsm->sdo->index); fp@436: return; fp@436: } fp@436: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: return; fp@436: } fp@436: fp@436: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_desc_response; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@2589: /** Prepare an entry description request. fp@2589: * fp@2589: * \return Zero on success, otherwise a negative error code. fp@2589: */ fp@2589: int ec_fsm_coe_dict_prepare_entry( fp@2589: ec_fsm_coe_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: u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); fp@2589: if (IS_ERR(data)) { fp@2589: return PTR_ERR(data); fp@2589: } fp@2589: fp@2589: EC_WRITE_U16(data, 0x8 << 12); // SDO information fp@2589: EC_WRITE_U8 (data + 2, 0x05); // Get entry description request fp@2589: EC_WRITE_U8 (data + 3, 0x00); fp@2589: EC_WRITE_U16(data + 4, 0x0000); fp@2589: EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index fp@2589: EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex fp@2589: EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only) fp@2589: fp@2589: fsm->state = ec_fsm_coe_dict_entry_request; fp@2589: return 0; fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@436: /** fp@436: CoE state: DICT DESC RESPONSE. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_desc_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@436: ec_sdo_t *sdo = fsm->sdo; fp@436: uint8_t *data, mbox_prot; fp@436: size_t rec_size, name_size; fp@436: 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@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" fp@1921: " response datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE SDO description" fp@1921: " response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1313: if (IS_ERR(data)) { fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: if (mbox_prot != 0x03) { // CoE fp@1921: EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", fp@1921: mbox_prot); fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@895: } fp@895: fp@895: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@895: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@895: fsm->retries = EC_FSM_RETRIES; fp@895: fsm->state = ec_fsm_coe_dict_desc_check; fp@895: return; fp@436: } fp@436: fp@1037: if (rec_size < 3) { fp@1921: EC_SLAVE_ERR(slave, "Received corrupted SDO description response" fp@1921: " (size %zu).\n", rec_size); fp@1037: fsm->state = ec_fsm_coe_error; fp@1037: return; fp@1037: } fp@1037: fp@1327: if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information fp@436: (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response fp@1921: EC_SLAVE_ERR(slave, "SDO information error response while" fp@1921: " fetching SDO 0x%04X!\n", sdo->index); fp@1921: ec_canopen_abort_msg(slave, EC_READ_U32(data + 6)); fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@436: } fp@436: fp@1037: if (rec_size < 8) { fp@1921: EC_SLAVE_ERR(slave, "Received corrupted SDO" fp@1921: " description response (size %zu).\n", rec_size); fp@1037: fsm->state = ec_fsm_coe_error; fp@1037: return; fp@1037: } fp@1037: fp@1327: if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information fp@436: (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response fp@1327: EC_READ_U16(data + 6) != sdo->index) { // SDO index fp@1053: if (fsm->slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Invalid object description response while" fp@1921: " fetching SDO 0x%04X!\n", sdo->index); fp@1053: ec_print_data(data, rec_size); fp@1053: } fp@1053: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1053: fsm->retries = EC_FSM_RETRIES; fp@1053: fsm->state = ec_fsm_coe_dict_desc_check; fp@895: return; fp@436: } fp@436: fp@436: if (rec_size < 12) { fp@1921: EC_SLAVE_ERR(slave, "Invalid data size!\n"); fp@436: ec_print_data(data, rec_size); fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@436: } fp@436: fp@962: sdo->max_subindex = EC_READ_U8(data + 10); fp@436: sdo->object_code = EC_READ_U8(data + 11); fp@436: fp@436: name_size = rec_size - 12; fp@436: if (name_size) { fp@998: if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) { fp@1921: EC_SLAVE_ERR(slave, "Failed to allocate SDO name!\n"); fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: memcpy(sdo->name, data + 12, name_size); fp@436: sdo->name[name_size] = 0; fp@436: } fp@436: fp@436: if (EC_READ_U8(data + 2) & 0x80) { fp@1921: EC_SLAVE_ERR(slave, "Fragment follows (not implemented)!\n"); fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@436: } fp@436: fp@436: // start fetching entries fp@436: fp@436: fsm->subindex = 0; fp@505: fsm->retries = EC_FSM_RETRIES; fp@2589: fp@2589: if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: DICT ENTRY REQUEST. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_entry_request( fp@2589: ec_fsm_coe_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: if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry" fp@1921: " request datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); 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@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_entry_check; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: DICT ENTRY CHECK. fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_entry_check( fp@2589: ec_fsm_coe_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@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" fp@1921: " datagram failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1038: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1053: if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fp@436: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Timeout while waiting for" fp@1921: " SDO entry 0x%04x:%x description response.\n", fp@1921: fsm->sdo->index, fsm->subindex); fp@436: return; fp@436: } fp@436: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: return; fp@436: } fp@436: fp@436: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_dict_entry_response; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: DICT ENTRY RESPONSE. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_dict_entry_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@436: ec_sdo_t *sdo = fsm->sdo; fp@436: uint8_t *data, mbox_prot; fp@436: size_t rec_size, data_size; fp@436: ec_sdo_entry_t *entry; fp@1382: u16 word; fp@436: 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@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" fp@1921: " description response datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE SDO description" fp@1921: " response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1313: if (IS_ERR(data)) { fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: if (mbox_prot != 0x03) { // CoE fp@1921: EC_SLAVE_ERR(slave, "Received mailbox protocol" fp@1921: " 0x%02X as response.\n", mbox_prot); fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@895: } fp@895: fp@895: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@895: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@895: fsm->retries = EC_FSM_RETRIES; fp@895: fsm->state = ec_fsm_coe_dict_entry_check; fp@895: return; fp@436: } fp@436: fp@1037: if (rec_size < 3) { fp@1921: EC_SLAVE_ERR(slave, "Received corrupted SDO entry" fp@1921: " description response (size %zu).\n", rec_size); fp@1037: fsm->state = ec_fsm_coe_error; fp@1037: return; fp@1037: } fp@1037: fp@1327: if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information fp@436: (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response fp@2195: EC_SLAVE_WARN(slave, "SDO information error response while" fp@1921: " fetching SDO entry 0x%04X:%02X!\n", fp@436: sdo->index, fsm->subindex); fp@1921: ec_canopen_abort_msg(slave, EC_READ_U32(data + 6)); fp@2195: fp@2195: /* There may be gaps in the subindices, so try to continue with next fp@2195: * subindex. */ fp@2195: fp@2195: } else { fp@2195: fp@2195: if (rec_size < 9) { fp@2195: EC_SLAVE_ERR(slave, "Received corrupted SDO entry" fp@2195: " description response (size %zu).\n", rec_size); fp@2195: fsm->state = ec_fsm_coe_error; fp@2195: return; fp@2195: } fp@2195: fp@2195: if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information fp@2589: (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response fp@2589: EC_READ_U16(data + 6) != sdo->index || // SDO index fp@2589: EC_READ_U8(data + 8) != fsm->subindex) { // SDO subindex fp@2195: if (fsm->slave->master->debug_level) { fp@2195: EC_SLAVE_DBG(slave, 1, "Invalid entry description response" fp@2195: " while fetching SDO entry 0x%04X:%02X!\n", fp@2195: sdo->index, fsm->subindex); fp@2195: ec_print_data(data, rec_size); fp@2195: } fp@2195: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@2195: fsm->retries = EC_FSM_RETRIES; fp@2195: fsm->state = ec_fsm_coe_dict_entry_check; fp@2195: return; fp@2195: } fp@2195: fp@2195: if (rec_size < 16) { fp@2195: EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size); fp@1053: ec_print_data(data, rec_size); fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@2195: fp@2195: data_size = rec_size - 16; fp@2195: fp@2195: if (!(entry = (ec_sdo_entry_t *) fp@2589: kmalloc(sizeof(ec_sdo_entry_t), GFP_KERNEL))) { fp@2195: EC_SLAVE_ERR(slave, "Failed to allocate entry!\n"); fp@2195: fsm->state = ec_fsm_coe_error; fp@2195: return; fp@2195: } fp@2195: fp@2195: ec_sdo_entry_init(entry, sdo, fsm->subindex); fp@2195: entry->data_type = EC_READ_U16(data + 10); fp@2195: entry->bit_length = EC_READ_U16(data + 12); fp@2195: fp@2195: // read access rights fp@2195: word = EC_READ_U16(data + 14); fp@2195: entry->read_access[EC_SDO_ENTRY_ACCESS_PREOP] = word & 0x0001; fp@2589: entry->read_access[EC_SDO_ENTRY_ACCESS_SAFEOP] = fp@2589: (word >> 1) & 0x0001; fp@2195: entry->read_access[EC_SDO_ENTRY_ACCESS_OP] = (word >> 2) & 0x0001; fp@2195: entry->write_access[EC_SDO_ENTRY_ACCESS_PREOP] = (word >> 3) & 0x0001; fp@2589: entry->write_access[EC_SDO_ENTRY_ACCESS_SAFEOP] = fp@2589: (word >> 4) & 0x0001; fp@2195: entry->write_access[EC_SDO_ENTRY_ACCESS_OP] = (word >> 5) & 0x0001; fp@2195: fp@2195: if (data_size) { fp@2195: uint8_t *desc; fp@2195: if (!(desc = kmalloc(data_size + 1, GFP_KERNEL))) { fp@2195: EC_SLAVE_ERR(slave, "Failed to allocate SDO entry name!\n"); fp@2195: fsm->state = ec_fsm_coe_error; fp@2195: return; fp@2195: } fp@2195: memcpy(desc, data + 16, data_size); fp@2195: desc[data_size] = 0; fp@2195: entry->description = desc; fp@2195: } fp@2195: fp@2195: list_add_tail(&entry->list, &sdo->entries); fp@2195: } fp@436: fp@962: if (fsm->subindex < sdo->max_subindex) { fp@2589: fp@436: fsm->subindex++; fp@2589: fsm->retries = EC_FSM_RETRIES; fp@2589: fp@2589: if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) { fp@436: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: fp@436: return; fp@436: } fp@436: fp@1327: // another SDO description to fetch? fp@436: if (fsm->sdo->list.next != &slave->sdo_dictionary) { fp@2589: fp@436: fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); fp@2589: fsm->retries = EC_FSM_RETRIES; fp@2589: fp@2589: if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) { fp@436: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: fp@436: return; fp@436: } fp@436: fp@436: fsm->state = ec_fsm_coe_end; fp@436: } fp@436: fp@436: /****************************************************************************** fp@436: * CoE state machine fp@436: *****************************************************************************/ fp@436: fp@2589: /** Prepare a donwnload request. fp@2589: * fp@2589: * \return Zero on success, otherwise a negative error code. fp@2589: */ fp@2589: int ec_fsm_coe_prepare_down_start( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@2589: u8 *data; fp@2589: ec_slave_t *slave = fsm->slave; fp@2589: ec_sdo_request_t *request = fsm->request; fp@2589: uint8_t data_set_size; fp@2589: fp@2589: if (request->data_size <= 4) { // use expedited transfer type fp@2589: data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, fp@2589: EC_COE_DOWN_REQ_HEADER_SIZE); fp@2589: if (IS_ERR(data)) { fp@2589: request->errno = PTR_ERR(data); fp@2589: return PTR_ERR(data); fp@2589: } fp@2589: fp@2589: fsm->remaining = 0; fp@2589: fp@2589: data_set_size = 4 - request->data_size; fp@2589: fp@2589: EC_WRITE_U16(data, 0x2 << 12); // SDO request fp@2589: EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited fp@2589: | data_set_size << 2 fp@2589: | ((request->complete_access ? 1 : 0) << 4) fp@2589: | 0x1 << 5)); // Download request fp@2589: EC_WRITE_U16(data + 3, request->index); fp@2589: EC_WRITE_U8 (data + 5, fp@2589: request->complete_access ? 0x00 : request->subindex); fp@2589: memcpy(data + 6, request->data, request->data_size); fp@2589: memset(data + 6 + request->data_size, 0x00, 4 - request->data_size); fp@2589: fp@2589: if (slave->master->debug_level) { fp@2589: EC_SLAVE_DBG(slave, 1, "Expedited download request:\n"); fp@2589: ec_print_data(data, EC_COE_DOWN_REQ_HEADER_SIZE); fp@2589: } fp@2589: } fp@2589: else { // request->data_size > 4, use normal transfer type fp@2589: size_t data_size, fp@2589: max_data_size = fp@2589: slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE, fp@2589: required_data_size = fp@2589: EC_COE_DOWN_REQ_HEADER_SIZE + request->data_size; fp@2589: fp@2589: if (max_data_size < required_data_size) { fp@2589: // segmenting needed fp@2589: data_size = max_data_size; fp@2589: } else { fp@2589: data_size = required_data_size; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, fp@2589: data_size); fp@2589: if (IS_ERR(data)) { fp@2589: request->errno = PTR_ERR(data); fp@2589: return PTR_ERR(data); fp@2589: } fp@2589: fp@2589: fsm->offset = 0; fp@2589: fsm->remaining = request->data_size; fp@2589: fp@2589: EC_WRITE_U16(data, 0x2 << 12); // SDO request fp@2589: EC_WRITE_U8(data + 2, fp@2589: 0x1 // size indicator, normal fp@2589: | ((request->complete_access ? 1 : 0) << 4) fp@2589: | 0x1 << 5); // Download request fp@2589: EC_WRITE_U16(data + 3, request->index); fp@2589: EC_WRITE_U8 (data + 5, fp@2589: request->complete_access ? 0x00 : request->subindex); fp@2589: EC_WRITE_U32(data + 6, request->data_size); fp@2589: fp@2589: if (data_size > EC_COE_DOWN_REQ_HEADER_SIZE) { fp@2589: size_t segment_size = data_size - EC_COE_DOWN_REQ_HEADER_SIZE; fp@2589: memcpy(data + EC_COE_DOWN_REQ_HEADER_SIZE, fp@2589: request->data, segment_size); fp@2589: fsm->offset += segment_size; fp@2589: fsm->remaining -= segment_size; fp@2589: } fp@2589: fp@2589: if (slave->master->debug_level) { fp@2589: EC_SLAVE_DBG(slave, 1, "Normal download request:\n"); fp@2589: ec_print_data(data, data_size); fp@2589: } fp@2589: } fp@2589: fp@2589: fsm->state = ec_fsm_coe_down_request; fp@2589: return 0; fp@2589: } fp@2589: fp@2589: /****************************************************************************/ fp@2589: fp@1563: /** CoE state: DOWN START. fp@1563: */ fp@1563: void ec_fsm_coe_down_start( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@854: ec_sdo_request_t *request = fsm->request; fp@436: fp@874: if (fsm->slave->master->debug_level) { fp@1526: char subidxstr[10]; fp@1526: if (request->complete_access) { fp@1526: subidxstr[0] = 0x00; fp@1526: } else { fp@1526: sprintf(subidxstr, ":%02X", request->subindex); fp@1526: } fp@1921: EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n", fp@1921: request->index, subidxstr); fp@874: ec_print_data(request->data, request->data_size); fp@874: } fp@436: fp@834: if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { fp@1921: EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); fp@1948: request->errno = EPROTONOSUPPORT; fp@825: fsm->state = ec_fsm_coe_error; fp@825: return; fp@825: } fp@1507: fp@2589: if (slave->configured_rx_mailbox_size < fp@1563: EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { fp@1921: EC_SLAVE_ERR(slave, "Mailbox too small!\n"); fp@1948: request->errno = EOVERFLOW; fp@1562: fsm->state = ec_fsm_coe_error; fp@1562: return; fp@1562: } fp@1562: fp@975: fp@1035: fsm->request->jiffies_sent = jiffies; fp@505: fsm->retries = EC_FSM_RETRIES; fp@2589: fp@2589: if (ec_fsm_coe_prepare_down_start(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: DOWN REQUEST. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_down_request( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@1792: unsigned long diff_ms; fp@436: fp@2589: if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { fp@2589: if (ec_fsm_coe_prepare_down_start(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE download" fp@1921: " request datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@505: return; fp@505: } fp@505: fp@1792: diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; fp@1792: fp@2589: if (fsm->datagram->working_counter != 1) { fp@2589: if (!fsm->datagram->working_counter) { fp@1035: if (diff_ms < fsm->request->response_timeout) { fp@1791: #if DEBUG_RETRIES fp@1931: EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO" fp@1931: " download request. Retrying after %lu ms...\n", fp@1931: diff_ms); fp@1791: #endif fp@1112: // no response; send request datagram again fp@2589: if (ec_fsm_coe_prepare_down_start(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@1112: return; fp@1035: } fp@1035: } fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE download request" fp@1931: " for SDO 0x%04x:%x failed with timeout after %lu ms: ", fp@1931: fsm->request->index, fsm->request->subindex, diff_ms); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@436: return; fp@436: } fp@436: fp@1792: #if DEBUG_LONG fp@1792: if (diff_ms > 200) { fp@1931: EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %lu ms.\n", fp@1931: fsm->request->index, fsm->request->subindex, diff_ms); fp@1792: } fp@1792: #endif fp@1792: fp@2589: fsm->jiffies_start = fsm->datagram->jiffies_sent; fp@2589: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_down_check; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@1907: /** CoE state: DOWN CHECK. fp@1907: */ fp@2589: void ec_fsm_coe_down_check( fp@2589: ec_fsm_coe_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@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" fp@1921: " datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" fp@1921: " datagram failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1038: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1053: if (diff_ms >= fsm->request->response_timeout) { fp@1948: fsm->request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@1931: EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" fp@1931: " for SDO 0x%04x:%x download response.\n", diff_ms, fp@1921: fsm->request->index, fsm->request->subindex); fp@436: return; fp@436: } fp@436: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: return; fp@436: } fp@436: fp@436: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_down_response; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@1907: /** Prepare a download segment request. fp@1907: */ fp@1563: void ec_fsm_coe_down_prepare_segment_request( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@1563: ec_slave_t *slave = fsm->slave; fp@1563: ec_sdo_request_t *request = fsm->request; fp@1563: size_t max_segment_size = fp@1563: slave->configured_rx_mailbox_size fp@1563: - EC_MBOX_HEADER_SIZE fp@1563: - EC_COE_DOWN_SEG_REQ_HEADER_SIZE; fp@2589: size_t data_size; fp@1563: uint8_t last_segment, seg_data_size, *data; fp@1563: fp@1563: if (fsm->remaining > max_segment_size) { fp@2589: fsm->segment_size = max_segment_size; fp@1563: last_segment = 0; fp@1563: } else { fp@2589: fsm->segment_size = fsm->remaining; fp@1563: last_segment = 1; fp@1563: } fp@1563: fp@2589: if (fsm->segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) { fp@1563: seg_data_size = 0x00; fp@2589: data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size; fp@1563: } else { fp@2589: seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size; fp@1563: data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE fp@1563: + EC_COE_DOWN_SEG_MIN_DATA_SIZE; fp@1563: } fp@1563: fp@2589: data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, fp@1563: data_size); fp@1563: if (IS_ERR(data)) { fp@1948: request->errno = PTR_ERR(data); fp@1563: fsm->state = ec_fsm_coe_error; fp@1563: return; fp@1563: } fp@1563: fp@1563: EC_WRITE_U16(data, 0x2 << 12); // SDO request fp@1563: EC_WRITE_U8(data + 2, (last_segment ? 1 : 0) fp@2589: | (seg_data_size << 1) fp@1563: | (fsm->toggle << 4) fp@1563: | (0x00 << 5)); // Download segment request fp@1563: memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE, fp@2589: request->data + fsm->offset, fsm->segment_size); fp@2589: if (fsm->segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) { fp@2589: memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size, fp@2589: 0x00, EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size); fp@2589: } fp@1563: fp@1563: if (slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Download segment request:\n"); fp@1563: ec_print_data(data, data_size); fp@1563: } fp@1563: fp@1563: fsm->state = ec_fsm_coe_down_seg_check; fp@1563: } fp@1563: fp@1563: /*****************************************************************************/ fp@1563: fp@436: /** fp@436: CoE state: DOWN RESPONSE. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_down_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@436: uint8_t *data, mbox_prot; fp@436: size_t rec_size; fp@854: ec_sdo_request_t *request = fsm->request; fp@436: 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@1948: request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE download" fp@1921: " response datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1313: if (IS_ERR(data)) { fp@1948: request->errno = PTR_ERR(data); fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: if (mbox_prot != 0x03) { // CoE fp@1948: request->errno = EIO; fp@860: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", fp@1921: mbox_prot); fp@860: return; fp@436: } fp@436: fp@895: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@895: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@895: fsm->retries = EC_FSM_RETRIES; fp@895: fsm->state = ec_fsm_coe_down_check; fp@895: return; fp@895: } fp@895: fp@975: if (slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Download response:\n"); fp@975: ec_print_data(data, rec_size); fp@975: } fp@975: fp@436: if (rec_size < 6) { fp@1948: request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n", fp@1921: rec_size); fp@436: ec_print_data(data, rec_size); fp@436: return; fp@436: } fp@436: fp@1327: if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request fp@1327: EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request fp@1526: char subidxstr[10]; fp@1948: request->errno = EIO; fp@1526: fsm->state = ec_fsm_coe_error; fp@1526: if (request->complete_access) { fp@1526: subidxstr[0] = 0x00; fp@1526: } else { fp@1526: sprintf(subidxstr, ":%02X", request->subindex); fp@1526: } fp@1921: EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n", fp@1921: request->index, subidxstr, request->data_size); fp@436: if (rec_size < 10) { fp@1921: EC_SLAVE_ERR(slave, "Incomplete abort command:\n"); fp@436: ec_print_data(data, rec_size); fp@1037: } else { fp@1037: fsm->request->abort_code = EC_READ_U32(data + 6); fp@1921: ec_canopen_abort_msg(slave, fsm->request->abort_code); fp@1037: } fp@436: return; fp@436: } fp@436: fp@1327: if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response fp@436: EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response fp@854: EC_READ_U16(data + 3) != request->index || // index fp@854: EC_READ_U8 (data + 5) != request->subindex) { // subindex fp@1053: if (slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" fp@1921: " Retrying...\n"); fp@1053: ec_print_data(data, rec_size); fp@1053: } fp@1053: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1053: fsm->retries = EC_FSM_RETRIES; fp@1053: fsm->state = ec_fsm_coe_down_check; fp@436: return; fp@436: } fp@436: fp@1562: if (fsm->remaining) { // more segments to download fp@1562: fsm->toggle = 0; fp@2589: ec_fsm_coe_down_prepare_segment_request(fsm, datagram); fp@1562: } else { fp@1562: fsm->state = ec_fsm_coe_end; // success fp@1562: } fp@1562: } fp@1562: fp@1562: /*****************************************************************************/ fp@1562: fp@1562: /** fp@1562: CoE state: DOWN SEG CHECK. fp@1562: */ fp@1562: fp@2589: void ec_fsm_coe_down_seg_check( fp@2589: ec_fsm_coe_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: return; fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@1948: fsm->request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: fsm->request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" fp@1921: " datagram failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1562: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1562: if (diff_ms >= fsm->request->response_timeout) { fp@1948: fsm->request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" fp@1921: " segment response.\n"); fp@1562: return; fp@1562: } fp@1562: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1562: fsm->retries = EC_FSM_RETRIES; fp@1562: return; fp@1562: } fp@1562: fp@1562: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@1562: fsm->retries = EC_FSM_RETRIES; fp@1562: fsm->state = ec_fsm_coe_down_seg_response; fp@1562: } fp@1562: fp@1562: /*****************************************************************************/ fp@1562: fp@1562: /** fp@1562: CoE state: DOWN SEG RESPONSE. fp@1562: \todo Timeout behavior fp@1562: */ fp@1562: fp@1562: void ec_fsm_coe_down_seg_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@1562: ec_slave_t *slave = fsm->slave; fp@1562: uint8_t *data, mbox_prot; fp@1562: size_t rec_size; fp@1562: ec_sdo_request_t *request = fsm->request; fp@1562: 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@1948: request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE download response" fp@1921: " datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1562: if (IS_ERR(data)) { fp@1948: request->errno = PTR_ERR(data); fp@1562: fsm->state = ec_fsm_coe_error; fp@1562: return; fp@1562: } fp@1562: fp@1562: if (mbox_prot != 0x03) { // CoE fp@1948: request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", fp@1921: mbox_prot); fp@1562: return; fp@1562: } fp@1562: fp@1562: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@1562: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1562: fsm->retries = EC_FSM_RETRIES; fp@1562: fsm->state = ec_fsm_coe_down_check; fp@1562: return; fp@1562: } fp@1562: fp@1562: if (slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Download response:\n"); fp@1562: ec_print_data(data, rec_size); fp@1562: } fp@1562: fp@1562: if (rec_size < 6) { fp@1948: request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n", fp@1921: rec_size); fp@1562: ec_print_data(data, rec_size); fp@1562: return; fp@1562: } fp@1562: fp@1562: if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request fp@1562: EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request fp@1562: char subidxstr[10]; fp@1948: request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1562: if (request->complete_access) { fp@1562: subidxstr[0] = 0x00; fp@1562: } else { fp@1562: sprintf(subidxstr, ":%02X", request->subindex); fp@1562: } fp@1921: EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n", fp@1921: request->index, subidxstr, request->data_size); fp@1562: if (rec_size < 10) { fp@1921: EC_SLAVE_ERR(slave, "Incomplete abort command:\n"); fp@1562: ec_print_data(data, rec_size); fp@1562: } else { fp@1562: fsm->request->abort_code = EC_READ_U32(data + 6); fp@1921: ec_canopen_abort_msg(slave, fsm->request->abort_code); fp@1562: } fp@1562: return; fp@1562: } fp@1562: fp@1562: if (EC_READ_U16(data) >> 12 != 0x3 || fp@1562: ((EC_READ_U8(data + 2) >> 5) != 0x01)) { // segment response fp@1562: if (slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" fp@1921: " Retrying...\n"); fp@1562: ec_print_data(data, rec_size); fp@1562: } fp@1562: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1562: fsm->retries = EC_FSM_RETRIES; fp@1562: fsm->state = ec_fsm_coe_down_seg_check; fp@1562: return; fp@1562: } fp@1562: fp@1562: if (((EC_READ_U8(data + 2) >> 4) & 0x01) != fsm->toggle) { fp@1921: EC_SLAVE_ERR(slave, "Invalid toggle received during" fp@1921: " segmented download:\n"); fp@1562: ec_print_data(data, rec_size); fp@1948: request->errno = EIO; fp@1562: fsm->state = ec_fsm_coe_error; fp@1562: return; fp@1562: } fp@1562: fp@2589: fsm->offset += fsm->segment_size; fp@2589: fsm->remaining -= fsm->segment_size; fp@2589: fp@1562: if (fsm->remaining) { // more segments to download fp@1562: fsm->toggle = !fsm->toggle; fp@2589: ec_fsm_coe_down_prepare_segment_request(fsm, datagram); fp@1562: } else { fp@1562: fsm->state = ec_fsm_coe_end; // success fp@1562: } fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@2589: /** Prepare an upload request. fp@2589: * fp@2589: * \return Zero on success, otherwise a negative error code. fp@2589: */ fp@2589: int ec_fsm_coe_prepare_up( fp@2589: ec_fsm_coe_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_sdo_request_t *request = fsm->request; fp@436: ec_master_t *master = slave->master; fp@2589: fp@2589: u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); fp@1313: if (IS_ERR(data)) { fp@1948: request->errno = PTR_ERR(data); fp@2589: return PTR_ERR(data); fp@436: } fp@436: fp@1327: EC_WRITE_U16(data, 0x2 << 12); // SDO request fp@436: EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request fp@831: EC_WRITE_U16(data + 3, request->index); fp@831: EC_WRITE_U8 (data + 5, request->subindex); fp@476: memset(data + 6, 0x00, 4); fp@436: fp@436: if (master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Upload request:\n"); fp@476: ec_print_data(data, 10); fp@436: } fp@436: fp@2589: fsm->state = ec_fsm_coe_up_request; fp@2589: return 0; fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@2589: fp@2589: /** fp@2589: CoE state: UP START. fp@2589: */ fp@2589: fp@2589: void ec_fsm_coe_up_start( fp@2589: ec_fsm_coe_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_sdo_request_t *request = fsm->request; fp@2589: fp@2589: EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n", fp@2589: request->index, request->subindex); fp@2589: fp@2589: if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { fp@2589: EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); fp@2589: request->errno = EPROTONOSUPPORT; fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: return; fp@2589: } fp@2589: fp@2589: fsm->retries = EC_FSM_RETRIES; fp@1035: fsm->request->jiffies_sent = jiffies; fp@2589: fp@2589: if (ec_fsm_coe_prepare_up(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: } fp@2589: fp@2589: /*****************************************************************************/ fp@436: /** fp@436: CoE state: UP REQUEST. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_up_request( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@1792: unsigned long diff_ms; fp@436: fp@2589: if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { fp@2589: if (ec_fsm_coe_prepare_up(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@505: return; fp@505: } fp@505: fp@1792: diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; fp@1792: fp@2589: if (fsm->datagram->working_counter != 1) { fp@2589: if (!fsm->datagram->working_counter) { fp@1035: if (diff_ms < fsm->request->response_timeout) { fp@1791: #if DEBUG_RETRIES fp@1921: EC_SLAVE_DBG(slave, 1, "Slave did not respond to" fp@1931: " SDO upload request. Retrying after %lu ms...\n", fp@1931: diff_ms); fp@1791: #endif fp@1112: // no response; send request datagram again fp@2589: if (ec_fsm_coe_prepare_up(fsm, datagram)) { fp@2589: fsm->state = ec_fsm_coe_error; fp@2589: } fp@1112: return; fp@1035: } fp@1035: } fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE upload request for" fp@1931: " SDO 0x%04x:%x failed with timeout after %lu ms: ", fp@1931: fsm->request->index, fsm->request->subindex, diff_ms); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@436: return; fp@436: } fp@436: fp@1792: #if DEBUG_LONG fp@1792: if (diff_ms > 200) { fp@1931: EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %lu ms.\n", fp@1931: fsm->request->index, fsm->request->subindex, diff_ms); fp@1792: } fp@1792: #endif fp@1792: fp@2589: fsm->jiffies_start = fsm->datagram->jiffies_sent; fp@2589: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_up_check; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: UP CHECK. fp@436: */ fp@436: fp@2589: void ec_fsm_coe_up_check( fp@2589: ec_fsm_coe_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@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" fp@1921: " datagram failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1038: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1043: if (diff_ms >= fsm->request->response_timeout) { fp@1948: fsm->request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@1931: EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" fp@1931: " SDO 0x%04x:%x upload response.\n", diff_ms, fp@1921: fsm->request->index, fsm->request->subindex); fp@436: return; fp@436: } fp@436: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: return; fp@436: } fp@436: fp@436: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_up_response; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@1498: /** Prepare an SDO upload segment request. fp@1498: */ fp@1498: void ec_fsm_coe_up_prepare_segment_request( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@1498: ) fp@1498: { fp@1498: uint8_t *data = fp@2589: ec_slave_mbox_prepare_send(fsm->slave, datagram, 0x03, 10); fp@1498: if (IS_ERR(data)) { fp@1948: fsm->request->errno = PTR_ERR(data); fp@1498: fsm->state = ec_fsm_coe_error; fp@1498: return; fp@1498: } fp@1498: fp@1498: EC_WRITE_U16(data, 0x2 << 12); // SDO request fp@1498: EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle fp@1498: | 0x3 << 5)); // upload segment request fp@1550: memset(data + 3, 0x00, 7); fp@1498: fp@1498: if (fsm->slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(fsm->slave, 1, "Upload segment request:\n"); fp@1498: ec_print_data(data, 10); fp@1498: } fp@1498: } fp@1498: fp@1498: /*****************************************************************************/ fp@1498: fp@436: /** fp@436: CoE state: UP RESPONSE. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_up_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@436: ec_master_t *master = slave->master; fp@1468: uint16_t rec_index; fp@1468: uint8_t *data, mbox_prot, rec_subindex; fp@436: size_t rec_size, data_size; fp@436: ec_sdo_request_t *request = fsm->request; fp@436: unsigned int expedited, size_specified; fp@1948: int ret; fp@436: 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@1948: request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" fp@1921: " datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1313: if (IS_ERR(data)) { fp@1948: request->errno = PTR_ERR(data); fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: if (master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Upload response:\n"); fp@436: ec_print_data(data, rec_size); fp@436: } fp@436: fp@436: if (mbox_prot != 0x03) { // CoE fp@1948: request->errno = EIO; fp@860: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X" fp@1921: " as response.\n", mbox_prot); fp@769: return; fp@769: } fp@769: fp@895: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@895: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@895: fsm->retries = EC_FSM_RETRIES; fp@895: fsm->state = ec_fsm_coe_up_check; fp@895: return; fp@895: } fp@895: fp@1468: if (rec_size < 6) { fp@1948: request->errno = EIO; fp@860: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received currupted SDO upload response" fp@1921: " (%zu bytes)!\n", rec_size); fp@436: ec_print_data(data, rec_size); fp@769: return; fp@436: } fp@436: fp@1327: if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request fp@1468: EC_READ_U8(data + 2) >> 5 == 0x4) { // abort SDO transfer request fp@1921: EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n", fp@1921: request->index, request->subindex); fp@1037: if (rec_size >= 10) { fp@1037: request->abort_code = EC_READ_U32(data + 6); fp@1921: ec_canopen_abort_msg(slave, request->abort_code); fp@1037: } else { fp@1921: EC_SLAVE_ERR(slave, "No abort message.\n"); fp@1037: } fp@1948: request->errno = EIO; fp@769: fsm->state = ec_fsm_coe_error; fp@769: return; fp@769: } fp@769: fp@1468: if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response fp@1468: EC_READ_U8(data + 2) >> 5 != 0x2) { // upload response fp@1921: EC_SLAVE_ERR(slave, "Received unknown response while" fp@1921: " uploading SDO 0x%04X:%02X.\n", fp@1921: request->index, request->subindex); fp@1468: ec_print_data(data, rec_size); fp@1948: request->errno = EIO; fp@1468: fsm->state = ec_fsm_coe_error; fp@1468: return; fp@1468: } fp@1468: fp@1468: rec_index = EC_READ_U16(data + 3); fp@1468: rec_subindex = EC_READ_U8(data + 5); fp@1468: fp@1468: if (rec_index != request->index || rec_subindex != request->subindex) { fp@1921: EC_SLAVE_ERR(slave, "Received upload response for wrong SDO" fp@1921: " (0x%04X:%02X, requested: 0x%04X:%02X).\n", fp@1921: rec_index, rec_subindex, request->index, request->subindex); fp@1468: ec_print_data(data, rec_size); fp@1468: fp@1468: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1468: fsm->retries = EC_FSM_RETRIES; fp@1468: fsm->state = ec_fsm_coe_up_check; fp@1468: return; fp@1468: } fp@1468: fp@769: // normal or expedited? fp@436: expedited = EC_READ_U8(data + 2) & 0x02; fp@436: fp@436: if (expedited) { fp@436: size_specified = EC_READ_U8(data + 2) & 0x01; fp@436: if (size_specified) { fp@1339: fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); fp@769: } else { fp@1339: fsm->complete_size = 4; fp@1339: } fp@1339: fp@1339: if (rec_size < 6 + fsm->complete_size) { fp@1948: request->errno = EIO; fp@860: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received corrupted SDO expedited upload" fp@1543: " response (only %zu bytes)!\n", rec_size); fp@769: ec_print_data(data, rec_size); fp@769: return; fp@769: } fp@769: fp@1948: ret = ec_sdo_request_copy_data(request, data + 6, fsm->complete_size); fp@1948: if (ret) { fp@1948: request->errno = -ret; fp@854: fsm->state = ec_fsm_coe_error; fp@854: return; fp@854: } fp@769: } else { // normal fp@769: if (rec_size < 10) { fp@1948: request->errno = EIO; fp@860: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Received currupted SDO normal upload" fp@1543: " response (only %zu bytes)!\n", rec_size); fp@769: ec_print_data(data, rec_size); fp@769: return; fp@769: } fp@769: fp@769: data_size = rec_size - 10; fp@1339: fsm->complete_size = EC_READ_U32(data + 6); fp@1339: fp@1339: if (!fsm->complete_size) { fp@1948: request->errno = EIO; fp@860: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "No complete size supplied!\n"); fp@769: ec_print_data(data, rec_size); fp@769: return; fp@769: } fp@769: fp@1948: ret = ec_sdo_request_alloc(request, fsm->complete_size); fp@1948: if (ret) { fp@1948: request->errno = -ret; fp@854: fsm->state = ec_fsm_coe_error; fp@854: return; fp@854: } fp@854: fp@1948: ret = ec_sdo_request_copy_data(request, data + 10, data_size); fp@1948: if (ret) { fp@1948: request->errno = -ret; fp@854: fsm->state = ec_fsm_coe_error; fp@854: return; fp@854: } fp@854: fp@436: fsm->toggle = 0; fp@436: fp@1339: if (data_size < fsm->complete_size) { fp@1921: EC_SLAVE_DBG(slave, 1, "SDO data incomplete (%zu / %u)." fp@1921: " Segmenting...\n", data_size, fsm->complete_size); fp@2589: ec_fsm_coe_up_prepare_segment_request(fsm, datagram); fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_up_seg_request; fp@436: return; fp@436: } fp@436: } fp@436: fp@874: if (master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Uploaded data:\n"); fp@874: ec_print_data(request->data, request->data_size); fp@874: } fp@874: fp@436: fsm->state = ec_fsm_coe_end; // success fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: UP REQUEST. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_up_seg_request( fp@2589: ec_fsm_coe_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_fsm_coe_up_prepare_segment_request(fsm, datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" fp@1921: " request datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE upload segment" fp@1921: " request failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); 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@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_up_seg_check; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: UP CHECK. fp@436: */ fp@436: fp@2589: void ec_fsm_coe_up_seg_check( fp@2589: ec_fsm_coe_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@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" fp@1921: " datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: fsm->request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" fp@1921: " failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (!ec_slave_mbox_check(fsm->datagram)) { fp@1038: unsigned long diff_ms = fp@2589: (fsm->datagram->jiffies_received - fsm->jiffies_start) * fp@2589: 1000 / HZ; fp@1053: if (diff_ms >= fsm->request->response_timeout) { fp@1948: fsm->request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" fp@1921: " segment response.\n"); fp@436: return; fp@436: } fp@436: fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: return; fp@436: } fp@436: fp@436: // Fetch response fp@2589: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_up_seg_response; fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: CoE state: UP RESPONSE. fp@505: \todo Timeout behavior fp@436: */ fp@436: fp@2589: void ec_fsm_coe_up_seg_response( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@436: ec_slave_t *slave = fsm->slave; fp@436: ec_master_t *master = slave->master; fp@436: uint8_t *data, mbox_prot; fp@436: size_t rec_size, data_size; fp@436: ec_sdo_request_t *request = fsm->request; fp@436: unsigned int last_segment; fp@436: 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@1948: request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" fp@1921: " response datagram: "); fp@2589: ec_datagram_print_state(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: if (fsm->datagram->working_counter != 1) { fp@1948: request->errno = EIO; fp@505: fsm->state = ec_fsm_coe_error; fp@1921: EC_SLAVE_ERR(slave, "Reception of CoE upload segment" fp@1921: " response failed: "); fp@2589: ec_datagram_print_wc_error(fsm->datagram); fp@2589: return; fp@2589: } fp@2589: fp@2589: data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); fp@1313: if (IS_ERR(data)) { fp@1948: request->errno = PTR_ERR(data); fp@436: fsm->state = ec_fsm_coe_error; fp@436: return; fp@436: } fp@436: fp@436: if (master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Upload segment response:\n"); fp@436: ec_print_data(data, rec_size); fp@436: } fp@436: fp@436: if (mbox_prot != 0x03) { // CoE fp@1921: EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", fp@1921: mbox_prot); fp@1948: request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@895: } fp@895: fp@895: if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { fp@895: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@895: fsm->retries = EC_FSM_RETRIES; fp@895: fsm->state = ec_fsm_coe_up_seg_check; fp@895: return; fp@436: } fp@436: fp@436: if (rec_size < 10) { fp@1921: EC_SLAVE_ERR(slave, "Received currupted SDO upload" fp@1921: " segment response!\n"); fp@436: ec_print_data(data, rec_size); fp@1948: request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@436: } fp@436: fp@1327: if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request fp@1468: EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request fp@1921: EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n", fp@1921: request->index, request->subindex); fp@1037: request->abort_code = EC_READ_U32(data + 6); fp@1921: ec_canopen_abort_msg(slave, request->abort_code); fp@1948: request->errno = EIO; fp@436: fsm->state = ec_fsm_coe_error; fp@895: return; fp@436: } fp@436: fp@1327: if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response fp@436: EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response fp@1053: if (fsm->slave->master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Invalid SDO upload segment response!\n"); fp@1053: ec_print_data(data, rec_size); fp@1053: } fp@1053: // check for CoE response again fp@2589: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@1053: fsm->retries = EC_FSM_RETRIES; fp@1053: fsm->state = ec_fsm_coe_up_seg_check; fp@895: return; fp@436: } fp@436: fp@1533: data_size = rec_size - 3; /* Header of segment upload is smaller than fp@1533: normal upload */ fp@1533: if (rec_size == 10) { fp@1533: uint8_t seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; fp@1533: data_size -= seg_size; fp@1339: } fp@1339: fp@1339: if (request->data_size + data_size > fsm->complete_size) { fp@1921: EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X failed: Fragment" fp@1339: " exceeding complete size!\n", fp@1921: request->index, request->subindex); fp@1948: request->errno = EOVERFLOW; fp@1339: fsm->state = ec_fsm_coe_error; fp@1339: return; fp@436: } fp@436: fp@1498: memcpy(request->data + request->data_size, data + 3, data_size); fp@854: request->data_size += data_size; fp@436: fp@1533: last_segment = EC_READ_U8(data + 2) & 0x01; fp@436: if (!last_segment) { fp@436: fsm->toggle = !fsm->toggle; fp@2589: ec_fsm_coe_up_prepare_segment_request(fsm, datagram); fp@505: fsm->retries = EC_FSM_RETRIES; fp@436: fsm->state = ec_fsm_coe_up_seg_request; fp@436: return; fp@436: } fp@436: fp@1339: if (request->data_size != fsm->complete_size) { fp@1921: EC_SLAVE_WARN(slave, "SDO upload 0x%04X:%02X: Assembled data" fp@1543: " size (%zu) does not match complete size (%u)!\n", fp@1921: request->index, request->subindex, fp@1339: request->data_size, fsm->complete_size); fp@1339: } fp@1339: fp@874: if (master->debug_level) { fp@1921: EC_SLAVE_DBG(slave, 1, "Uploaded data:\n"); fp@874: ec_print_data(request->data, request->data_size); fp@874: } fp@874: fp@436: fsm->state = ec_fsm_coe_end; // success fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: State: ERROR. fp@436: */ fp@436: fp@2589: void ec_fsm_coe_error( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@436: { fp@436: } fp@436: fp@436: /*****************************************************************************/ fp@436: fp@436: /** fp@436: State: END. fp@436: */ fp@436: fp@2589: void ec_fsm_coe_end( fp@2589: ec_fsm_coe_t *fsm, /**< Finite state machine. */ fp@2589: ec_datagram_t *datagram /**< Datagram to use. */ fp@2589: ) fp@2589: { fp@2589: } fp@2589: fp@2589: /*****************************************************************************/