fp@1732: /****************************************************************************** fp@1732: * fp@1732: * $Id$ fp@1732: * fp@1732: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@1732: * fp@1732: * This file is part of the IgH EtherCAT Master. fp@1732: * fp@1732: * The IgH EtherCAT Master is free software; you can redistribute it fp@1732: * and/or modify it under the terms of the GNU General Public License fp@1732: * as published by the Free Software Foundation; either version 2 of the fp@1732: * License, or (at your option) any later version. fp@1732: * fp@1732: * The IgH EtherCAT Master is distributed in the hope that it will be fp@1732: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1732: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@1732: * GNU General Public License for more details. fp@1732: * fp@1732: * You should have received a copy of the GNU General Public License fp@1732: * along with the IgH EtherCAT Master; if not, write to the Free Software fp@1732: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@1732: * fp@1732: * The right to use EtherCAT Technology is granted and comes free of fp@1732: * charge under condition of compatibility of product made by fp@1732: * Licensee. People intending to distribute/sell products based on the fp@1732: * code, have to sign an agreement to guarantee that products using fp@1732: * software based on IgH EtherCAT master stay compatible with the actual fp@1732: * EtherCAT specification (which are released themselves as an open fp@1732: * standard) as the (only) precondition to have the right to use EtherCAT fp@1732: * Technology, IP and trade marks. fp@1732: * fp@1732: *****************************************************************************/ fp@1732: fp@1732: /** fp@1732: \file fp@1732: EtherCAT state change FSM. fp@1732: */ fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: #include "globals.h" fp@1732: #include "master.h" fp@1732: #include "fsm_change.h" fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: void ec_fsm_change_state_start(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_check(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_status(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_code(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_start_ack(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_ack(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_check_ack(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_end(ec_fsm_change_t *); fp@1732: void ec_fsm_change_state_error(ec_fsm_change_t *); fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Constructor. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_init(ec_fsm_change_t *fsm, /**< finite state machine */ fp@1732: ec_datagram_t *datagram /**< datagram */ fp@1732: ) fp@1732: { fp@1732: fsm->state = NULL; fp@1732: fsm->datagram = datagram; fp@1746: fsm->spontaneous_change = 0; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Destructor. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_clear(ec_fsm_change_t *fsm /**< finite state machine */) fp@1732: { fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Starts the change state machine. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_start(ec_fsm_change_t *fsm, /**< finite state machine */ fp@1732: ec_slave_t *slave, /**< EtherCAT slave */ fp@1732: ec_slave_state_t state /**< requested state */ fp@1732: ) fp@1732: { fp@1732: fsm->mode = EC_FSM_CHANGE_MODE_FULL; fp@1732: fsm->slave = slave; fp@1732: fsm->requested_state = state; fp@1732: fsm->state = ec_fsm_change_state_start; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Starts the change state machine to only acknowlegde a slave's state. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_ack(ec_fsm_change_t *fsm, /**< finite state machine */ fp@1732: ec_slave_t *slave /**< EtherCAT slave */ fp@1732: ) fp@1732: { fp@1732: fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY; fp@1732: fsm->slave = slave; fp@1732: fsm->requested_state = EC_SLAVE_STATE_UNKNOWN; fp@1732: fsm->state = ec_fsm_change_state_start_ack; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Executes the current state of the state machine. fp@1732: \return false, if the state machine has terminated fp@1732: */ fp@1732: fp@1732: int ec_fsm_change_exec(ec_fsm_change_t *fsm /**< finite state machine */) fp@1732: { fp@1732: fsm->state(fsm); fp@1732: fp@1732: return fsm->state != ec_fsm_change_state_end fp@1732: && fsm->state != ec_fsm_change_state_error; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Returns, if the state machine terminated with success. fp@1732: \return non-zero if successful. fp@1732: */ fp@1732: fp@1732: int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */) fp@1732: { fp@1732: return fsm->state == ec_fsm_change_state_end; fp@1732: } fp@1732: fp@1732: /****************************************************************************** fp@1732: * state change state machine fp@1732: *****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: START. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_start(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: ec_slave_t *slave = fsm->slave; fp@1732: fp@1732: fsm->take_time = 1; fp@1732: fsm->old_state = fsm->slave->current_state; fp@1732: fp@1732: // write new state to slave fp@1732: ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); fp@1732: EC_WRITE_U16(datagram->data, fsm->requested_state); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: fsm->state = ec_fsm_change_state_check; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: CHECK. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_check(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: ec_slave_t *slave = fsm->slave; fp@1732: fp@1744: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1744: return; fp@1739: fp@1732: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1732: fsm->state = ec_fsm_change_state_error; fp@1744: EC_ERR("Failed to receive state datagram from slave %i" fp@1744: " (datagram state %i)!\n", fp@1744: fsm->slave->ring_position, datagram->state); fp@1732: return; fp@1732: } fp@1732: fp@1732: if (fsm->take_time) { fp@1732: fsm->take_time = 0; fp@1732: fsm->jiffies_start = datagram->jiffies_sent; fp@1732: } fp@1732: fp@1745: if (datagram->working_counter == 0) { fp@1732: if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) { fp@1732: char state_str[EC_STATE_STRING_SIZE]; fp@1732: ec_state_string(fsm->requested_state, state_str); fp@1732: fsm->state = ec_fsm_change_state_error; fp@1745: EC_ERR("Failed to set state %s on slave %i: ", fp@1745: state_str, fsm->slave->ring_position); fp@1745: ec_datagram_print_wc_error(datagram); fp@1732: return; fp@1732: } fp@1732: fp@1732: // repeat writing new state to slave fp@1732: ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); fp@1732: EC_WRITE_U16(datagram->data, fsm->requested_state); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: return; fp@1732: } fp@1732: fp@1745: if (unlikely(datagram->working_counter > 1)) { fp@1745: char state_str[EC_STATE_STRING_SIZE]; fp@1745: ec_state_string(fsm->requested_state, state_str); fp@1745: fsm->state = ec_fsm_change_state_error; fp@1745: EC_ERR("Failed to set state %s on slave %i: ", fp@1745: state_str, fsm->slave->ring_position); fp@1745: ec_datagram_print_wc_error(datagram); fp@1745: return; fp@1745: } fp@1745: fp@1732: fsm->take_time = 1; fp@1732: fp@1732: // read AL status from slave fp@1732: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1746: fsm->spontaneous_change = 0; fp@1732: fsm->state = ec_fsm_change_state_status; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: STATUS. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_status(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: ec_slave_t *slave = fsm->slave; fp@1732: fp@1744: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1744: return; fp@1739: fp@1739: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1739: fsm->state = ec_fsm_change_state_error; fp@1744: EC_ERR("Failed to receive state checking datagram from slave %i" fp@1744: " (datagram state %i).\n", fp@1744: slave->ring_position, datagram->state); fp@1739: return; fp@1739: } fp@1739: fp@1739: if (datagram->working_counter != 1) { fp@1732: char req_state[EC_STATE_STRING_SIZE]; fp@1732: ec_state_string(fsm->requested_state, req_state); fp@1732: fsm->state = ec_fsm_change_state_error; fp@1745: EC_ERR("Failed to check state %s on slave %i: ", fp@1732: req_state, slave->ring_position); fp@1745: ec_datagram_print_wc_error(datagram); fp@1732: return; fp@1732: } fp@1732: fp@1732: if (fsm->take_time) { fp@1732: fsm->take_time = 0; fp@1732: fsm->jiffies_start = datagram->jiffies_sent; fp@1732: } fp@1732: fp@1732: slave->current_state = EC_READ_U8(datagram->data); fp@1732: fp@1732: if (slave->current_state == fsm->requested_state) { fp@1732: // state has been set successfully fp@1732: fsm->state = ec_fsm_change_state_end; fp@1732: return; fp@1732: } fp@1732: fp@1732: if (slave->current_state != fsm->old_state) { // state changed fp@1732: char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE]; fp@1732: fp@1732: ec_state_string(slave->current_state, cur_state); fp@1732: fp@1732: if ((slave->current_state & 0x0F) != (fsm->old_state & 0x0F)) { fp@1732: // Slave spontaneously changed its state just before the new state fp@1732: // was written. Accept current state as old state and wait for fp@1732: // state change fp@1746: fsm->spontaneous_change = 1; fp@1732: fsm->old_state = slave->current_state; fp@1732: EC_WARN("Slave %i changed to %s in the meantime.\n", fp@1732: slave->ring_position, cur_state); fp@1746: goto check_again; fp@1732: } fp@1732: fp@1732: // state change error fp@1732: fp@1732: slave->error_flag = 1; fp@1732: ec_state_string(fsm->requested_state, req_state); fp@1732: fp@1732: EC_ERR("Failed to set %s state, slave %i refused state change (%s).\n", fp@1732: req_state, slave->ring_position, cur_state); fp@1732: // fetch AL status error code fp@1732: ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: fsm->state = ec_fsm_change_state_code; fp@1732: return; fp@1732: } fp@1732: fp@1732: // still old state fp@1732: fp@1732: if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s fp@1732: // timeout while checking fp@1732: char state_str[EC_STATE_STRING_SIZE]; fp@1732: ec_state_string(fsm->requested_state, state_str); fp@1732: fsm->state = ec_fsm_change_state_error; fp@1732: EC_ERR("Timeout while setting state %s on slave %i.\n", fp@1746: state_str, slave->ring_position); fp@1746: return; fp@1746: } fp@1746: fp@1746: check_again: fp@1732: // no timeout yet. check again fp@1732: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Application layer status messages. fp@1732: */ fp@1732: fp@1732: const ec_code_msg_t al_status_messages[] = { fp@1732: {0x0001, "Unspecified error"}, fp@1732: {0x0011, "Invalud requested state change"}, fp@1732: {0x0012, "Unknown requested state"}, fp@1732: {0x0013, "Bootstrap not supported"}, fp@1732: {0x0014, "No valid firmware"}, fp@1732: {0x0015, "Invalid mailbox configuration"}, fp@1732: {0x0016, "Invalid mailbox configuration"}, fp@1732: {0x0017, "Invalid sync manager configuration"}, fp@1732: {0x0018, "No valid inputs available"}, fp@1732: {0x0019, "No valid outputs"}, fp@1732: {0x001A, "Synchronisation error"}, fp@1732: {0x001B, "Sync manager watchdog"}, fp@1732: {0x001C, "Invalid sync manager types"}, fp@1732: {0x001D, "Invalid output configuration"}, fp@1732: {0x001E, "Invalid input configuration"}, fp@1732: {0x001F, "Invalid watchdog configuration"}, fp@1732: {0x0020, "Slave needs cold start"}, fp@1732: {0x0021, "Slave needs INIT"}, fp@1732: {0x0022, "Slave needs PREOP"}, fp@1732: {0x0023, "Slave needs SAVEOP"}, fp@1732: {0x0030, "Invalid DC SYNCH configuration"}, fp@1732: {0x0031, "Invalid DC latch configuration"}, fp@1732: {0x0032, "PLL error"}, fp@1732: {0x0033, "Invalid DC IO error"}, fp@1732: {0x0034, "Invalid DC timeout error"}, fp@1732: {0x0042, "MBOX EOE"}, fp@1732: {0x0043, "MBOX COE"}, fp@1732: {0x0044, "MBOX FOE"}, fp@1732: {0x0045, "MBOX SOE"}, fp@1732: {0x004F, "MBOX VOE"}, fp@1732: {} fp@1732: }; fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: CODE. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_code(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: uint32_t code; fp@1732: const ec_code_msg_t *al_msg; fp@1732: fp@1744: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1744: return; fp@1739: fp@1739: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1739: fsm->state = ec_fsm_change_state_error; fp@1744: EC_ERR("Failed to receive AL status code datagram from slave %i" fp@1744: " (datagram state %i).\n", fp@1744: fsm->slave->ring_position, datagram->state); fp@1739: return; fp@1739: } fp@1739: fp@1739: if (datagram->working_counter != 1) { fp@1745: EC_WARN("Reception of AL status code datagram failed: "); fp@1745: ec_datagram_print_wc_error(datagram); fp@1732: } fp@1732: else { fp@1732: if ((code = EC_READ_U16(datagram->data))) { fp@1732: for (al_msg = al_status_messages; al_msg->code; al_msg++) { fp@1732: if (al_msg->code != code) continue; fp@1732: EC_ERR("AL status message 0x%04X: \"%s\".\n", fp@1732: al_msg->code, al_msg->message); fp@1732: break; fp@1732: } fp@1732: if (!al_msg->code) fp@1732: EC_ERR("Unknown AL status code 0x%04X.\n", code); fp@1732: } fp@1732: } fp@1732: fp@1732: // acknowledge "old" slave state fp@1732: ec_fsm_change_state_start_ack(fsm); // execute immediately fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: START ACK. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: ec_slave_t *slave = fsm->slave; fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: fp@1732: ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); fp@1732: EC_WRITE_U16(datagram->data, slave->current_state); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: fsm->state = ec_fsm_change_state_ack; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: ACK. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */) fp@1732: { fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: ec_slave_t *slave = fsm->slave; fp@1732: fp@1744: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1744: return; fp@1739: fp@1739: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1739: fsm->state = ec_fsm_change_state_error; fp@1744: EC_ERR("Failed to receive state ack datagram for slave %i" fp@1744: " (datagram state %i).\n", fp@1744: slave->ring_position, datagram->state); fp@1739: return; fp@1739: } fp@1739: fp@1739: if (datagram->working_counter != 1) { fp@1739: fsm->state = ec_fsm_change_state_error; fp@1745: EC_ERR("Reception of state ack datagram failed on slave %i: ", fp@1745: slave->ring_position); fp@1745: ec_datagram_print_wc_error(datagram); fp@1732: return; fp@1732: } fp@1732: fp@1732: fsm->take_time = 1; fp@1732: fp@1732: // read new AL status fp@1732: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: fsm->state = ec_fsm_change_state_check_ack; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: Change state: CHECK ACK. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: ec_datagram_t *datagram = fsm->datagram; fp@1732: ec_slave_t *slave = fsm->slave; fp@1732: fp@1744: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1744: return; fp@1739: fp@1739: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1739: fsm->state = ec_fsm_change_state_error; fp@1744: EC_ERR("Failed to receive state ack check datagram from slave %i" fp@1744: " (datagram state %i).\n", fp@1744: slave->ring_position, datagram->state); fp@1739: return; fp@1739: } fp@1739: fp@1739: if (datagram->working_counter != 1) { fp@1739: fsm->state = ec_fsm_change_state_error; fp@1745: EC_ERR("Reception of state ack check datagram failed on slave %i: ", fp@1745: slave->ring_position); fp@1745: ec_datagram_print_wc_error(datagram); fp@1732: return; fp@1732: } fp@1732: fp@1732: if (fsm->take_time) { fp@1732: fsm->take_time = 0; fp@1732: fsm->jiffies_start = datagram->jiffies_sent; fp@1732: } fp@1732: fp@1732: slave->current_state = EC_READ_U8(datagram->data); fp@1732: fp@1732: if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) { fp@1732: char state_str[EC_STATE_STRING_SIZE]; fp@1732: ec_state_string(slave->current_state, state_str); fp@1732: if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) { fp@1732: fsm->state = ec_fsm_change_state_error; fp@1732: } fp@1732: else { // EC_FSM_CHANGE_MODE_ACK_ONLY fp@1732: fsm->state = ec_fsm_change_state_end; fp@1732: } fp@1732: EC_INFO("Acknowledged state %s on slave %i.\n", fp@1732: state_str, slave->ring_position); fp@1732: return; fp@1732: } fp@1732: fp@1732: if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s fp@1732: // timeout while checking fp@1732: char state_str[EC_STATE_STRING_SIZE]; fp@1732: ec_state_string(slave->current_state, state_str); fp@1732: fsm->state = ec_fsm_change_state_error; fp@1732: EC_ERR("Timeout while acknowledging state %s on slave %i.\n", fp@1732: state_str, slave->ring_position); fp@1732: return; fp@1732: } fp@1732: fp@1732: // reread new AL status fp@1732: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@1739: fsm->retries = EC_FSM_RETRIES; fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: State: ERROR. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_error(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: } fp@1732: fp@1732: /*****************************************************************************/ fp@1732: fp@1732: /** fp@1732: State: END. fp@1732: */ fp@1732: fp@1732: void ec_fsm_change_state_end(ec_fsm_change_t *fsm fp@1732: /**< finite state machine */) fp@1732: { fp@1732: } fp@1732: fp@1732: /*****************************************************************************/