martin@1583: /****************************************************************************** martin@1583: * fp@2099: * $Id$ martin@1583: * fp@2443: * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH martin@1583: * martin@1583: * This file is part of the IgH EtherCAT Master. martin@1583: * martin@1583: * The IgH EtherCAT Master is free software; you can redistribute it and/or martin@1583: * modify it under the terms of the GNU General Public License version 2, as martin@1583: * published by the Free Software Foundation. martin@1583: * martin@1583: * The IgH EtherCAT Master is distributed in the hope that it will be useful, martin@1583: * but WITHOUT ANY WARRANTY; without even the implied warranty of martin@1583: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General martin@1583: * Public License for more details. martin@1583: * martin@1583: * You should have received a copy of the GNU General Public License along martin@1583: * with the IgH EtherCAT Master; if not, write to the Free Software martin@1583: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA martin@1583: * martin@1583: * --- martin@1583: * martin@1583: * The license mentioned above concerns the source code only. Using the martin@1583: * EtherCAT technology and brand is only permitted in compliance with the martin@1583: * industrial property and similar rights of Beckhoff Automation GmbH. martin@1583: * martin@1583: *****************************************************************************/ martin@1583: martin@1583: /** \file martin@1583: * EtherCAT slave (SDO) state machine. martin@1583: */ martin@1583: martin@1583: /*****************************************************************************/ martin@1583: martin@1583: #include "globals.h" martin@1583: #include "master.h" martin@1583: #include "mailbox.h" fp@2443: #include "slave_config.h" martin@1583: martin@1583: #include "fsm_slave.h" martin@1583: martin@1583: /*****************************************************************************/ martin@1583: martin@1583: void ec_fsm_slave_state_idle(ec_fsm_slave_t *); martin@1601: void ec_fsm_slave_state_ready(ec_fsm_slave_t *); martin@1597: int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *); fp@1831: void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); fp@2443: int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *); fp@2443: void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *); martin@1597: int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *); martin@1597: void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *); fp@1831: int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *); fp@1831: void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *); martin@1583: martin@1583: /*****************************************************************************/ martin@1583: martin@1583: /** Constructor. martin@1583: */ martin@1583: void ec_fsm_slave_init( martin@1583: ec_fsm_slave_t *fsm, /**< Slave state machine. */ martin@1583: ec_slave_t *slave, /**< EtherCAT slave. */ martin@1583: ec_datagram_t *datagram /**< Datagram object to use. */ martin@1583: ) martin@1583: { martin@1583: fsm->slave = slave; martin@1583: fsm->datagram = datagram; martin@1583: fsm->datagram->data_size = 0; fp@1804: fp@1921: EC_SLAVE_DBG(slave, 1, "Init FSM.\n"); fp@1804: fp@1804: fsm->state = ec_fsm_slave_state_idle; martin@1583: martin@1583: // init sub-state-machines martin@1583: ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); martin@1597: ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); fp@1831: ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram); martin@1583: } martin@1583: martin@1583: /*****************************************************************************/ martin@1583: martin@1583: /** Destructor. martin@1583: */ martin@1583: void ec_fsm_slave_clear( martin@1583: ec_fsm_slave_t *fsm /**< Master state machine. */ martin@1583: ) martin@1583: { martin@1583: // clear sub-state machines martin@1583: ec_fsm_coe_clear(&fsm->fsm_coe); martin@1597: ec_fsm_foe_clear(&fsm->fsm_foe); fp@1831: ec_fsm_soe_clear(&fsm->fsm_soe); martin@1583: } martin@1583: martin@1583: /*****************************************************************************/ martin@1583: martin@1583: /** Executes the current state of the state machine. martin@1583: * martin@1583: * If the state machine's datagram is not sent or received yet, the execution martin@1583: * of the state machine is delayed to the next cycle. martin@1583: */ martin@1583: void ec_fsm_slave_exec( martin@1583: ec_fsm_slave_t *fsm /**< Slave state machine. */ martin@1583: ) martin@1583: { martin@1583: if (fsm->datagram->state == EC_DATAGRAM_SENT martin@1583: || fsm->datagram->state == EC_DATAGRAM_QUEUED) { martin@1583: // datagram was not sent or received yet. martin@1583: return; martin@1583: } martin@1583: martin@1583: fsm->state(fsm); fp@2440: fp@2440: ec_datagram_output_stats(fsm->datagram); martin@1583: } martin@1583: martin@1601: /*****************************************************************************/ martin@1601: martin@1601: /** Sets the current state of the state machine to READY martin@1601: */ martin@1601: void ec_fsm_slave_ready( fp@1804: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@1804: ) fp@1804: { fp@1804: if (fsm->state == ec_fsm_slave_state_idle) { fp@1921: EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n"); fp@1804: fsm->state = ec_fsm_slave_state_ready; fp@1804: } martin@1601: } martin@1601: martin@1583: /****************************************************************************** martin@1583: * Slave state machine martin@1583: *****************************************************************************/ martin@1583: martin@1583: /** Slave state: IDLE. martin@1583: */ martin@1583: void ec_fsm_slave_state_idle( fp@1804: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@1804: ) fp@1804: { fp@1804: // do nothing martin@1601: } martin@1601: martin@1601: /*****************************************************************************/ martin@1601: martin@1601: /** Slave state: READY. martin@1601: */ martin@1601: void ec_fsm_slave_state_ready( fp@1804: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@1804: ) fp@1804: { fp@1804: // Check for pending external SDO requests fp@2304: if (ec_fsm_slave_action_process_sdo(fsm)) { fp@2304: return; fp@2304: } fp@1804: fp@2443: // Check for pending external register requests fp@2443: if (ec_fsm_slave_action_process_reg(fsm)) { fp@2443: return; fp@2443: } fp@2443: fp@1831: // Check for pending FoE requests fp@2304: if (ec_fsm_slave_action_process_foe(fsm)) { fp@2304: return; fp@2304: } fp@1831: fp@1831: // Check for pending SoE requests fp@2304: if (ec_fsm_slave_action_process_soe(fsm)) { fp@2304: return; fp@2304: } fp@1804: } martin@1597: martin@1597: /*****************************************************************************/ martin@1597: martin@1597: /** Check for pending SDO requests and process one. martin@1597: * martin@1597: * \return non-zero, if an SDO request is processed. martin@1597: */ martin@1597: int ec_fsm_slave_action_process_sdo( martin@1597: ec_fsm_slave_t *fsm /**< Slave state machine. */ martin@1597: ) martin@1597: { martin@1583: ec_slave_t *slave = fsm->slave; fp@2467: ec_sdo_request_t *request; fp@2467: fp@2467: if (list_empty(&slave->sdo_requests)) { fp@2467: return 0; fp@2467: } fp@2467: fp@2467: // take the first request to be processed fp@2467: request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list); fp@2467: list_del_init(&request->list); // dequeue fp@2467: fp@2467: if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { fp@2467: EC_SLAVE_WARN(slave, "Aborting SDO request," fp@2467: " slave has error flag set.\n"); fp@2467: request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->state = ec_fsm_slave_state_idle; martin@1597: return 1; martin@1597: } fp@2467: fp@2467: if (slave->current_state == EC_SLAVE_STATE_INIT) { fp@2467: EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); fp@2467: request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->state = ec_fsm_slave_state_idle; fp@2467: return 1; fp@2467: } fp@2467: fp@2467: request->state = EC_INT_REQUEST_BUSY; fp@2467: fp@2467: // Found pending SDO request. Execute it! fp@2467: EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); fp@2467: fp@2467: // Start SDO transfer fp@2467: fsm->sdo_request = request; fp@2467: fsm->state = ec_fsm_slave_state_sdo_request; fp@2467: ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request); fp@2467: ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately fp@2467: ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); fp@2467: return 1; martin@1597: } martin@1597: martin@1583: /*****************************************************************************/ martin@1583: martin@1583: /** Slave state: SDO_REQUEST. martin@1583: */ martin@1583: void ec_fsm_slave_state_sdo_request( martin@1583: ec_fsm_slave_t *fsm /**< Slave state machine. */ martin@1583: ) martin@1583: { martin@1583: ec_slave_t *slave = fsm->slave; martin@1583: ec_sdo_request_t *request = fsm->sdo_request; martin@1583: fp@2268: if (ec_fsm_coe_exec(&fsm->fsm_coe)) { fp@2374: ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); martin@1583: return; martin@1583: } fp@2443: martin@1583: if (!ec_fsm_coe_success(&fsm->fsm_coe)) { fp@1921: EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); martin@1583: request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@1804: fsm->sdo_request = NULL; fp@2442: fsm->state = ec_fsm_slave_state_ready; martin@1583: return; martin@1583: } martin@1583: fp@1921: EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); martin@1597: martin@1583: // SDO request finished martin@1583: request->state = EC_INT_REQUEST_SUCCESS; fp@2467: wake_up(&slave->master->request_queue); martin@1597: fsm->sdo_request = NULL; fp@1804: fsm->state = ec_fsm_slave_state_ready; fp@1804: } martin@1597: martin@1597: /*****************************************************************************/ martin@1597: fp@2443: /** Check for pending register requests and process one. fp@2443: * fp@2443: * \return non-zero, if a register request is processed. fp@2443: */ fp@2443: int ec_fsm_slave_action_process_reg( fp@2443: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@2443: ) fp@2443: { fp@2443: ec_slave_t *slave = fsm->slave; fp@2462: ec_reg_request_t *reg; fp@2443: fp@2443: fsm->reg_request = NULL; fp@2443: fp@2443: if (slave->config) { fp@2443: // search the first internal register request to be processed fp@2443: list_for_each_entry(reg, &slave->config->reg_requests, list) { fp@2443: if (reg->state == EC_INT_REQUEST_QUEUED) { fp@2443: fsm->reg_request = reg; fp@2443: break; fp@2443: } fp@2443: } fp@2443: } fp@2443: fp@2462: if (!fsm->reg_request && !list_empty(&slave->reg_requests)) { fp@2462: // take the first external request to be processed fp@2462: fsm->reg_request = fp@2462: list_entry(slave->reg_requests.next, ec_reg_request_t, list); fp@2462: list_del_init(&fsm->reg_request->list); // dequeue fp@2443: } fp@2443: fp@2443: if (!fsm->reg_request) { // no register request to process fp@2443: return 0; fp@2443: } fp@2443: fp@2443: if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { fp@2443: EC_SLAVE_WARN(slave, "Aborting register request," fp@2443: " slave has error flag set.\n"); fp@2462: fsm->reg_request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->reg_request = NULL; fp@2443: fsm->state = ec_fsm_slave_state_idle; fp@2443: return 1; fp@2443: } fp@2443: fp@2443: // Found pending register request. Execute it! fp@2443: EC_SLAVE_DBG(slave, 1, "Processing register request...\n"); fp@2443: fp@2462: fsm->reg_request->state = EC_INT_REQUEST_BUSY; fp@2443: fp@2443: // Start register access fp@2462: if (fsm->reg_request->dir == EC_DIR_INPUT) { fp@2443: ec_datagram_fprd(fsm->datagram, slave->station_address, fp@2462: fsm->reg_request->address, fsm->reg_request->transfer_size); fp@2443: ec_datagram_zero(fsm->datagram); fp@2443: } else { fp@2443: ec_datagram_fpwr(fsm->datagram, slave->station_address, fp@2462: fsm->reg_request->address, fsm->reg_request->transfer_size); fp@2462: memcpy(fsm->datagram->data, fsm->reg_request->data, fp@2462: fsm->reg_request->transfer_size); fp@2443: } fp@2443: fsm->datagram->device_index = slave->device_index; fp@2443: ec_master_queue_external_datagram(slave->master, fsm->datagram); fp@2443: fsm->state = ec_fsm_slave_state_reg_request; fp@2443: return 1; fp@2443: } fp@2443: fp@2443: /*****************************************************************************/ fp@2443: fp@2443: /** Slave state: Register request. fp@2443: */ fp@2443: void ec_fsm_slave_state_reg_request( fp@2443: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@2443: ) fp@2443: { fp@2443: ec_slave_t *slave = fsm->slave; fp@2443: ec_reg_request_t *reg = fsm->reg_request; fp@2443: fp@2459: if (!reg) { fp@2459: // configuration was cleared in the meantime fp@2459: fsm->state = ec_fsm_slave_state_ready; fp@2459: return; fp@2459: } fp@2459: fp@2443: if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fp@2443: EC_SLAVE_ERR(slave, "Failed to receive register" fp@2443: " request datagram: "); fp@2443: ec_datagram_print_state(fsm->datagram); fp@2443: reg->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->reg_request = NULL; fp@2443: fsm->state = ec_fsm_slave_state_ready; fp@2443: return; fp@2443: } fp@2443: fp@2443: if (fsm->datagram->working_counter == 1) { fp@2443: if (reg->dir == EC_DIR_INPUT) { // read request fp@2443: memcpy(reg->data, fsm->datagram->data, reg->transfer_size); fp@2443: } fp@2443: fp@2443: reg->state = EC_INT_REQUEST_SUCCESS; fp@2443: EC_SLAVE_DBG(slave, 1, "Register request successful.\n"); fp@2443: } else { fp@2443: reg->state = EC_INT_REQUEST_FAILURE; fp@2443: ec_datagram_print_state(fsm->datagram); fp@2443: EC_SLAVE_ERR(slave, "Register request failed" fp@2443: " (working counter is %u).\n", fp@2443: fsm->datagram->working_counter); fp@2443: } fp@2443: fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->reg_request = NULL; fp@2443: fsm->state = ec_fsm_slave_state_ready; fp@2443: } fp@2443: fp@2443: /*****************************************************************************/ fp@2443: fp@2442: /** Check for pending FoE requests and process one. fp@2442: * fp@2442: * \return non-zero, if an FoE request is processed. fp@1831: */ fp@1831: int ec_fsm_slave_action_process_foe( fp@1831: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@1831: ) fp@1831: { fp@1831: ec_slave_t *slave = fsm->slave; fp@2464: ec_foe_request_t *request; fp@2464: fp@2464: if (list_empty(&slave->foe_requests)) { fp@2464: return 0; fp@2464: } fp@2464: fp@2464: // take the first request to be processed fp@2464: request = list_entry(slave->foe_requests.next, ec_foe_request_t, list); fp@2464: list_del_init(&request->list); // dequeue fp@2464: fp@2464: if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { fp@2464: EC_SLAVE_WARN(slave, "Aborting FoE request," fp@2464: " slave has error flag set.\n"); fp@2464: request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->state = ec_fsm_slave_state_idle; fp@2467: return 1; fp@2464: } fp@2464: fp@2464: request->state = EC_INT_REQUEST_BUSY; fp@2464: fp@2464: EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); fp@2464: fp@2464: fsm->foe_request = request; fp@2464: fsm->state = ec_fsm_slave_state_foe_request; fp@2464: ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request); fp@2464: ec_fsm_foe_exec(&fsm->fsm_foe); fp@2464: ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); fp@2464: return 1; fp@1831: } fp@1831: fp@1831: /*****************************************************************************/ fp@1831: martin@1597: /** Slave state: FOE REQUEST. martin@1597: */ martin@1597: void ec_fsm_slave_state_foe_request( martin@1597: ec_fsm_slave_t *fsm /**< Slave state machine. */ martin@1597: ) martin@1597: { martin@1597: ec_slave_t *slave = fsm->slave; martin@1597: ec_foe_request_t *request = fsm->foe_request; martin@1597: fp@2268: if (ec_fsm_foe_exec(&fsm->fsm_foe)) { fp@2374: ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); martin@1597: return; martin@1597: } martin@1597: martin@1597: if (!ec_fsm_foe_success(&fsm->fsm_foe)) { fp@1921: EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); martin@1597: request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); martin@1597: fsm->foe_request = NULL; fp@2442: fsm->state = ec_fsm_slave_state_ready; martin@1597: return; martin@1597: } martin@1597: martin@1597: // finished transferring FoE fp@1930: EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE" fp@1921: " data.\n", request->data_size); martin@1597: martin@1597: request->state = EC_INT_REQUEST_SUCCESS; fp@2467: wake_up(&slave->master->request_queue); martin@1597: fsm->foe_request = NULL; fp@1804: fsm->state = ec_fsm_slave_state_ready; fp@1804: } fp@1804: fp@1804: /*****************************************************************************/ fp@1831: fp@1831: /** Check for pending SoE requests and process one. fp@1831: * fp@1831: * \return non-zero, if a request is processed. fp@1831: */ fp@1831: int ec_fsm_slave_action_process_soe( fp@1831: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@1831: ) fp@1831: { fp@1831: ec_slave_t *slave = fsm->slave; fp@2465: ec_soe_request_t *req; fp@2465: fp@2465: if (list_empty(&slave->soe_requests)) { fp@2465: return 0; fp@2465: } fp@2465: fp@2465: // take the first request to be processed fp@2465: req = list_entry(slave->soe_requests.next, ec_soe_request_t, list); fp@2465: list_del_init(&req->list); // dequeue fp@2465: fp@2465: if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { fp@2465: EC_SLAVE_WARN(slave, "Aborting SoE request," fp@2465: " slave has error flag set.\n"); fp@2465: req->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->state = ec_fsm_slave_state_idle; fp@2467: return 1; fp@2465: } fp@2465: fp@2465: if (slave->current_state == EC_SLAVE_STATE_INIT) { fp@2465: EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); fp@2465: req->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@2467: fsm->state = ec_fsm_slave_state_idle; fp@2465: return 0; fp@2465: } fp@2465: fp@2465: req->state = EC_INT_REQUEST_BUSY; fp@2465: fp@2465: // Found pending request. Execute it! fp@2465: EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n"); fp@2465: fp@2465: // Start SoE transfer fp@2465: fsm->soe_request = req; fp@2465: fsm->state = ec_fsm_slave_state_soe_request; fp@2465: ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req); fp@2465: ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately fp@2465: ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); fp@2465: return 1; fp@1831: } fp@1831: fp@1831: /*****************************************************************************/ fp@1831: fp@1831: /** Slave state: SOE_REQUEST. fp@1831: */ fp@1831: void ec_fsm_slave_state_soe_request( fp@1831: ec_fsm_slave_t *fsm /**< Slave state machine. */ fp@1831: ) fp@1831: { fp@1831: ec_slave_t *slave = fsm->slave; fp@1831: ec_soe_request_t *request = fsm->soe_request; fp@1831: fp@1831: if (ec_fsm_soe_exec(&fsm->fsm_soe)) { fp@2374: ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); fp@1831: return; fp@1831: } fp@1831: fp@1831: if (!ec_fsm_soe_success(&fsm->fsm_soe)) { fp@1921: EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); fp@1831: request->state = EC_INT_REQUEST_FAILURE; fp@2467: wake_up(&slave->master->request_queue); fp@1831: fsm->soe_request = NULL; fp@2442: fsm->state = ec_fsm_slave_state_ready; fp@1831: return; fp@1831: } fp@1831: fp@1921: EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); fp@1831: fp@1831: // SoE request finished fp@1831: request->state = EC_INT_REQUEST_SUCCESS; fp@2467: wake_up(&slave->master->request_queue); fp@1831: fsm->soe_request = NULL; fp@1831: fsm->state = ec_fsm_slave_state_ready; fp@1831: } fp@1831: fp@1831: /*****************************************************************************/