# HG changeset patch # User Florian Pose # Date 1161611109 0 # Node ID 0180d82773118fe2ce6e8765c8a44506e94c90b4 # Parent 100f51f28cf2b86cd70f3c50989ffe16a406f625 Layed out state change state machine. diff -r 100f51f28cf2 -r 0180d8277311 master/Kbuild --- a/master/Kbuild Mon Oct 23 12:59:42 2006 +0000 +++ b/master/Kbuild Mon Oct 23 13:45:09 2006 +0000 @@ -39,7 +39,7 @@ ec_master-objs := module.o master.o device.o slave.o datagram.o \ domain.o mailbox.o canopen.o ethernet.o \ - fsm_sii.o fsm.o xmldev.o + fsm_sii.o fsm_change.o fsm.o xmldev.o ifeq ($(EC_DBG_IF),1) ec_master-objs += debug.o diff -r 100f51f28cf2 -r 0180d8277311 master/Makefile.am --- a/master/Makefile.am Mon Oct 23 12:59:42 2006 +0000 +++ b/master/Makefile.am Mon Oct 23 13:45:09 2006 +0000 @@ -41,6 +41,7 @@ doxygen.c \ ethernet.c ethernet.h \ fsm_sii.c fsm_sii.h \ + fsm_change.c fsm_change.h \ fsm.c fsm.h \ globals.h \ mailbox.c mailbox.h \ diff -r 100f51f28cf2 -r 0180d8277311 master/fsm.c --- a/master/fsm.c Mon Oct 23 12:59:42 2006 +0000 +++ b/master/fsm.c Mon Oct 23 13:45:09 2006 +0000 @@ -80,13 +80,6 @@ void ec_fsm_slaveconf_saveop(ec_fsm_t *); void ec_fsm_slaveconf_op(ec_fsm_t *); -void ec_fsm_change_start(ec_fsm_t *); -void ec_fsm_change_check(ec_fsm_t *); -void ec_fsm_change_status(ec_fsm_t *); -void ec_fsm_change_code(ec_fsm_t *); -void ec_fsm_change_ack(ec_fsm_t *); -void ec_fsm_change_check_ack(ec_fsm_t *); - void ec_fsm_coe_dict_start(ec_fsm_t *); void ec_fsm_coe_dict_request(ec_fsm_t *); void ec_fsm_coe_dict_check(ec_fsm_t *); @@ -139,6 +132,7 @@ } ec_fsm_sii_init(&fsm->fsm_sii, &fsm->datagram); + ec_fsm_change_init(&fsm->fsm_change, &fsm->datagram); return 0; } @@ -152,6 +146,7 @@ void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */) { ec_fsm_sii_clear(&fsm->fsm_sii); + ec_fsm_change_clear(&fsm->fsm_change); ec_datagram_clear(&fsm->datagram); } @@ -389,8 +384,7 @@ // begin configuring slaves fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); fsm->slave_state = ec_fsm_slaveconf_init; - fsm->change_new = EC_SLAVE_STATE_INIT; - fsm->change_state = ec_fsm_change_start; + ec_fsm_change(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); fsm->master_state = ec_fsm_configuration_conf; fsm->master_state(fsm); // execute immediately } @@ -419,8 +413,7 @@ if (slave->list.next != &master->slaves) { fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fsm->slave_state = ec_fsm_slaveconf_init; - fsm->change_new = EC_SLAVE_STATE_INIT; - fsm->change_state = ec_fsm_change_start; + ec_fsm_change(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); fsm->master_state(fsm); // execute immediately return; } @@ -601,8 +594,7 @@ fsm->slave = slave; fsm->slave_state = ec_fsm_slaveconf_init; - fsm->change_new = EC_SLAVE_STATE_INIT; - fsm->change_state = ec_fsm_change_start; + ec_fsm_change(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); fsm->master_state = ec_fsm_master_configure_slave; fsm->master_state(fsm); // execute immediately return; @@ -1454,16 +1446,16 @@ const ec_sii_sync_t *sync; ec_sii_sync_t mbox_sync; - fsm->change_state(fsm); // execute state change state machine - - if (fsm->change_state == ec_fsm_error) { + ec_fsm_change_exec(&fsm->fsm_change); // execute state change state machine + + if (ec_fsm_change_running(&fsm->fsm_change)) return; + + if (!ec_fsm_change_success(&fsm->fsm_change)) { slave->error_flag = 1; fsm->slave_state = ec_fsm_error; return; } - if (fsm->change_state != ec_fsm_end) return; - slave->configured = 1; if (master->debug_level) { @@ -1486,9 +1478,8 @@ if (!slave->base_sync_count) { // no sync managers fsm->slave_state = ec_fsm_slaveconf_preop; - fsm->change_new = EC_SLAVE_STATE_PREOP; - fsm->change_state = ec_fsm_change_start; - fsm->change_state(fsm); // execute immediately + ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_PREOP); + ec_fsm_change_exec(&fsm->fsm_change); // execute immediately return; } @@ -1563,9 +1554,8 @@ } fsm->slave_state = ec_fsm_slaveconf_preop; - fsm->change_new = EC_SLAVE_STATE_PREOP; - fsm->change_state = ec_fsm_change_start; - fsm->change_state(fsm); // execute immediately + ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_PREOP); + ec_fsm_change_exec(&fsm->fsm_change); // execute immediately } /*****************************************************************************/ @@ -1581,16 +1571,16 @@ ec_datagram_t *datagram = &fsm->datagram; unsigned int j; - fsm->change_state(fsm); // execute state change state machine - - if (fsm->change_state == ec_fsm_error) { + ec_fsm_change_exec(&fsm->fsm_change); // execute state change state machine + + if (ec_fsm_change_running(&fsm->fsm_change)) return; + + if (!ec_fsm_change_success(&fsm->fsm_change)) { slave->error_flag = 1; fsm->slave_state = ec_fsm_error; return; } - if (fsm->change_state != ec_fsm_end) return; - // slave is now in PREOP slave->jiffies_preop = fsm->datagram.jiffies_received; @@ -1610,9 +1600,8 @@ if (!slave->base_fmmu_count) { // skip FMMU configuration if (list_empty(&slave->sdo_confs)) { // skip SDO configuration fsm->slave_state = ec_fsm_slaveconf_saveop; - fsm->change_new = EC_SLAVE_STATE_SAVEOP; - fsm->change_state = ec_fsm_change_start; - fsm->change_state(fsm); // execute immediately + ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_SAVEOP); + ec_fsm_change_exec(&fsm->fsm_change); // execute immediately return; } @@ -1661,9 +1650,8 @@ if (list_empty(&slave->sdo_confs)) { // skip SDO configuration // set state to SAVEOP fsm->slave_state = ec_fsm_slaveconf_saveop; - fsm->change_new = EC_SLAVE_STATE_SAVEOP; - fsm->change_state = ec_fsm_change_start; - fsm->change_state(fsm); // execute immediately + ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_SAVEOP); + ec_fsm_change_exec(&fsm->fsm_change); // execute immediately return; } @@ -1705,9 +1693,8 @@ // set state to SAVEOP fsm->slave_state = ec_fsm_slaveconf_saveop; - fsm->change_new = EC_SLAVE_STATE_SAVEOP; - fsm->change_state = ec_fsm_change_start; - fsm->change_state(fsm); // execute immediately + ec_fsm_change(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAVEOP); + ec_fsm_change_exec(&fsm->fsm_change); // execute immediately } /*****************************************************************************/ @@ -1721,16 +1708,16 @@ ec_master_t *master = fsm->master; ec_slave_t *slave = fsm->slave; - fsm->change_state(fsm); // execute state change state machine - - if (fsm->change_state == ec_fsm_error) { + ec_fsm_change_exec(&fsm->fsm_change); // execute state change state machine + + if (ec_fsm_change_running(&fsm->fsm_change)) return; + + if (!ec_fsm_change_success(&fsm->fsm_change)) { fsm->slave->error_flag = 1; fsm->slave_state = ec_fsm_error; return; } - if (fsm->change_state != ec_fsm_end) return; - // slave is now in SAVEOP if (master->debug_level) { @@ -1748,9 +1735,8 @@ // set state to OP fsm->slave_state = ec_fsm_slaveconf_op; - fsm->change_new = EC_SLAVE_STATE_OP; - fsm->change_state = ec_fsm_change_start; - fsm->change_state(fsm); // execute immediately + ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP); + ec_fsm_change_exec(&fsm->fsm_change); // execute immediately } /*****************************************************************************/ @@ -1764,16 +1750,16 @@ ec_master_t *master = fsm->master; ec_slave_t *slave = fsm->slave; - fsm->change_state(fsm); // execute state change state machine - - if (fsm->change_state == ec_fsm_error) { + ec_fsm_change_exec(&fsm->fsm_change); // execute state change state machine + + if (ec_fsm_change_running(&fsm->fsm_change)) return; + + if (!ec_fsm_change_success(&fsm->fsm_change)) { slave->error_flag = 1; fsm->slave_state = ec_fsm_error; return; } - if (fsm->change_state != ec_fsm_end) return; - // slave is now in OP if (master->debug_level) { @@ -1785,282 +1771,6 @@ } /****************************************************************************** - * state change state machine - *****************************************************************************/ - -/** - Change state: START. -*/ - -void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_datagram_t *datagram = &fsm->datagram; - ec_slave_t *slave = fsm->slave; - - fsm->change_take_time = 1; - - // write new state to slave - ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); - EC_WRITE_U16(datagram->data, fsm->change_new); - ec_master_queue_datagram(fsm->master, datagram); - fsm->change_state = ec_fsm_change_check; -} - -/*****************************************************************************/ - -/** - Change state: CHECK. -*/ - -void ec_fsm_change_check(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_datagram_t *datagram = &fsm->datagram; - ec_slave_t *slave = fsm->slave; - - if (datagram->state != EC_DATAGRAM_RECEIVED) { - fsm->change_state = ec_fsm_error; - EC_ERR("Failed to send state datagram to slave %i!\n", - fsm->slave->ring_position); - return; - } - - if (fsm->change_take_time) { - fsm->change_take_time = 0; - fsm->change_jiffies = datagram->jiffies_sent; - } - - if (datagram->working_counter != 1) { - if (datagram->jiffies_received - fsm->change_jiffies >= 3 * HZ) { - fsm->change_state = ec_fsm_error; - EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" - " respond.\n", fsm->change_new, fsm->slave->ring_position); - return; - } - - // repeat writing new state to slave - ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); - EC_WRITE_U16(datagram->data, fsm->change_new); - ec_master_queue_datagram(fsm->master, datagram); - return; - } - - fsm->change_take_time = 1; - - // read AL status from slave - ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); - ec_master_queue_datagram(fsm->master, datagram); - fsm->change_state = ec_fsm_change_status; -} - -/*****************************************************************************/ - -/** - Change state: STATUS. -*/ - -void ec_fsm_change_status(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_datagram_t *datagram = &fsm->datagram; - ec_slave_t *slave = fsm->slave; - - if (datagram->state != EC_DATAGRAM_RECEIVED - || datagram->working_counter != 1) { - fsm->change_state = ec_fsm_error; - EC_ERR("Failed to check state 0x%02X on slave %i.\n", - fsm->change_new, slave->ring_position); - return; - } - - if (fsm->change_take_time) { - fsm->change_take_time = 0; - fsm->change_jiffies = datagram->jiffies_sent; - } - - slave->current_state = EC_READ_U8(datagram->data); - - if (slave->current_state == fsm->change_new) { - // state has been set successfully - fsm->change_state = ec_fsm_end; - return; - } - - if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { - // state change error - EC_ERR("Failed to set state 0x%02X - Slave %i refused state change" - " (code 0x%02X)!\n", fsm->change_new, slave->ring_position, - slave->current_state); - // fetch AL status error code - ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); - ec_master_queue_datagram(fsm->master, datagram); - fsm->change_state = ec_fsm_change_code; - return; - } - - if (datagram->jiffies_received - - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms - // timeout while checking - fsm->change_state = ec_fsm_error; - EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", - fsm->change_new, slave->ring_position); - return; - } - - // still old state: check again - ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); - ec_master_queue_datagram(fsm->master, datagram); -} - -/*****************************************************************************/ - -/** - Application layer status messages. -*/ - -const ec_code_msg_t al_status_messages[] = { - {0x0001, "Unspecified error"}, - {0x0011, "Invalud requested state change"}, - {0x0012, "Unknown requested state"}, - {0x0013, "Bootstrap not supported"}, - {0x0014, "No valid firmware"}, - {0x0015, "Invalid mailbox configuration"}, - {0x0016, "Invalid mailbox configuration"}, - {0x0017, "Invalid sync manager configuration"}, - {0x0018, "No valid inputs available"}, - {0x0019, "No valid outputs"}, - {0x001A, "Synchronisation error"}, - {0x001B, "Sync manager watchdog"}, - {0x001C, "Invalid sync manager types"}, - {0x001D, "Invalid output configuration"}, - {0x001E, "Invalid input configuration"}, - {0x001F, "Invalid watchdog configuration"}, - {0x0020, "Slave needs cold start"}, - {0x0021, "Slave needs INIT"}, - {0x0022, "Slave needs PREOP"}, - {0x0023, "Slave needs SAVEOP"}, - {0x0030, "Invalid DC SYNCH configuration"}, - {0x0031, "Invalid DC latch configuration"}, - {0x0032, "PLL error"}, - {0x0033, "Invalid DC IO error"}, - {0x0034, "Invalid DC timeout error"}, - {0x0042, "MBOX EOE"}, - {0x0043, "MBOX COE"}, - {0x0044, "MBOX FOE"}, - {0x0045, "MBOX SOE"}, - {0x004F, "MBOX VOE"}, - {} -}; - -/*****************************************************************************/ - -/** - Change state: CODE. -*/ - -void ec_fsm_change_code(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_datagram_t *datagram = &fsm->datagram; - ec_slave_t *slave = fsm->slave; - uint32_t code; - const ec_code_msg_t *al_msg; - - if (datagram->state != EC_DATAGRAM_RECEIVED - || datagram->working_counter != 1) { - EC_WARN("Reception of AL status code datagram failed.\n"); - } - else { - if ((code = EC_READ_U16(datagram->data))) { - for (al_msg = al_status_messages; al_msg->code; al_msg++) { - if (al_msg->code != code) continue; - EC_ERR("AL status message 0x%04X: \"%s\".\n", - al_msg->code, al_msg->message); - break; - } - if (!al_msg->code) - EC_ERR("Unknown AL status code 0x%04X.\n", code); - } - } - - // acknowledge "old" slave state - ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); - EC_WRITE_U16(datagram->data, slave->current_state); - ec_master_queue_datagram(fsm->master, datagram); - fsm->change_state = ec_fsm_change_ack; -} - -/*****************************************************************************/ - -/** - Change state: ACK. -*/ - -void ec_fsm_change_ack(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_datagram_t *datagram = &fsm->datagram; - ec_slave_t *slave = fsm->slave; - - if (datagram->state != EC_DATAGRAM_RECEIVED - || datagram->working_counter != 1) { - fsm->change_state = ec_fsm_error; - EC_ERR("Reception of state ack datagram failed.\n"); - return; - } - - fsm->change_take_time = 1; - - // read new AL status - ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); - ec_master_queue_datagram(fsm->master, datagram); - fsm->change_state = ec_fsm_change_check_ack; -} - -/*****************************************************************************/ - -/** - Change state: CHECK ACK. -*/ - -void ec_fsm_change_check_ack(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_datagram_t *datagram = &fsm->datagram; - ec_slave_t *slave = fsm->slave; - - if (datagram->state != EC_DATAGRAM_RECEIVED - || datagram->working_counter != 1) { - fsm->change_state = ec_fsm_error; - EC_ERR("Reception of state ack check datagram failed.\n"); - return; - } - - if (fsm->change_take_time) { - fsm->change_take_time = 0; - fsm->change_jiffies = datagram->jiffies_sent; - } - - slave->current_state = EC_READ_U8(datagram->data); - - if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) { - fsm->change_state = ec_fsm_error; - EC_INFO("Acknowledged state 0x%02X on slave %i.\n", - slave->current_state, slave->ring_position); - return; - } - - if (datagram->jiffies_received - - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms - // timeout while checking - slave->current_state = EC_SLAVE_STATE_UNKNOWN; - fsm->change_state = ec_fsm_error; - EC_ERR("Timeout while acknowledging state 0x%02X on slave %i.\n", - fsm->change_new, slave->ring_position); - return; - } - - // reread new AL status - ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); - ec_master_queue_datagram(fsm->master, datagram); -} - -/****************************************************************************** * CoE dictionary state machine *****************************************************************************/ diff -r 100f51f28cf2 -r 0180d8277311 master/fsm.h --- a/master/fsm.h Mon Oct 23 12:59:42 2006 +0000 +++ b/master/fsm.h Mon Oct 23 13:45:09 2006 +0000 @@ -48,6 +48,7 @@ #include "canopen.h" #include "fsm_sii.h" +#include "fsm_change.h" /*****************************************************************************/ @@ -72,11 +73,7 @@ void (*slave_state)(ec_fsm_t *); /**< slave state function */ ec_fsm_sii_t fsm_sii; /**< SII state machine */ - - void (*change_state)(ec_fsm_t *); /**< slave state change state function */ - ec_slave_state_t change_new; /**< input: new state */ - unsigned long change_jiffies; /**< change timer */ - uint8_t change_take_time; /**< take sending timestamp */ + ec_fsm_change_t fsm_change; /**< State change state machine */ void (*coe_state)(ec_fsm_t *); /**< CoE state function */ ec_sdo_data_t *coe_sdodata; /**< input/output: SDO data object */ diff -r 100f51f28cf2 -r 0180d8277311 master/fsm_change.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_change.c Mon Oct 23 13:45:09 2006 +0000 @@ -0,0 +1,429 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it + * and/or modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * The IgH EtherCAT Master is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * The right to use EtherCAT Technology is granted and comes free of + * charge under condition of compatibility of product made by + * Licensee. People intending to distribute/sell products based on the + * code, have to sign an agreement to guarantee that products using + * software based on IgH EtherCAT master stay compatible with the actual + * EtherCAT specification (which are released themselves as an open + * standard) as the (only) precondition to have the right to use EtherCAT + * Technology, IP and trade marks. + * + *****************************************************************************/ + +/** + \file + EtherCAT state change FSM. +*/ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "fsm_change.h" + +/*****************************************************************************/ + +void ec_fsm_change_start(ec_fsm_change_t *); +void ec_fsm_change_check(ec_fsm_change_t *); +void ec_fsm_change_status(ec_fsm_change_t *); +void ec_fsm_change_code(ec_fsm_change_t *); +void ec_fsm_change_ack(ec_fsm_change_t *); +void ec_fsm_change_check_ack(ec_fsm_change_t *); +void ec_fsm_change_end(ec_fsm_change_t *); +void ec_fsm_change_error(ec_fsm_change_t *); + +/*****************************************************************************/ + +/** + Constructor. +*/ + +void ec_fsm_change_init(ec_fsm_change_t *fsm, /**< finite state machine */ + ec_datagram_t *datagram /**< datagram */ + ) +{ + fsm->state = NULL; + fsm->datagram = datagram; +} + +/*****************************************************************************/ + +/** + Destructor. +*/ + +void ec_fsm_change_clear(ec_fsm_change_t *fsm /**< finite state machine */) +{ +} + +/*****************************************************************************/ + +/** + Resets the state machine. +*/ + +void ec_fsm_change(ec_fsm_change_t *fsm, /**< finite state machine */ + ec_slave_t *slave, /**< EtherCAT slave */ + ec_slave_state_t state /**< requested state */ + ) +{ + fsm->slave = slave; + fsm->requested_state = state; + fsm->state = ec_fsm_change_start; +} + +/*****************************************************************************/ + +/** + Executes the current state of the state machine. +*/ + +void ec_fsm_change_exec(ec_fsm_change_t *fsm /**< finite state machine */) +{ + fsm->state(fsm); +} + +/*****************************************************************************/ + +/** + Returns the running state of the state machine. + \return non-zero if not terminated yet. +*/ + +int ec_fsm_change_running(ec_fsm_change_t *fsm /**< Finite state machine */) +{ + return fsm->state != ec_fsm_change_end + && fsm->state != ec_fsm_change_error; +} + +/*****************************************************************************/ + +/** + Returns, if the state machine terminated with success. + \return non-zero if successful. +*/ + +int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */) +{ + return fsm->state == ec_fsm_change_end; +} + +/****************************************************************************** + * state change state machine + *****************************************************************************/ + +/** + Change state: START. +*/ + +void ec_fsm_change_start(ec_fsm_change_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + fsm->take_time = 1; + + // write new state to slave + ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); + EC_WRITE_U16(datagram->data, fsm->requested_state); + ec_master_queue_datagram(fsm->slave->master, datagram); + fsm->state = ec_fsm_change_check; +} + +/*****************************************************************************/ + +/** + Change state: CHECK. +*/ + +void ec_fsm_change_check(ec_fsm_change_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_change_error; + EC_ERR("Failed to send state datagram to slave %i!\n", + fsm->slave->ring_position); + return; + } + + if (fsm->take_time) { + fsm->take_time = 0; + fsm->jiffies_start = datagram->jiffies_sent; + } + + if (datagram->working_counter != 1) { + if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) { + fsm->state = ec_fsm_change_error; + EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" + " respond.\n", fsm->requested_state, + fsm->slave->ring_position); + return; + } + + // repeat writing new state to slave + ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); + EC_WRITE_U16(datagram->data, fsm->requested_state); + ec_master_queue_datagram(fsm->slave->master, datagram); + return; + } + + fsm->take_time = 1; + + // read AL status from slave + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->slave->master, datagram); + fsm->state = ec_fsm_change_status; +} + +/*****************************************************************************/ + +/** + Change state: STATUS. +*/ + +void ec_fsm_change_status(ec_fsm_change_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->state = ec_fsm_change_error; + EC_ERR("Failed to check state 0x%02X on slave %i.\n", + fsm->requested_state, slave->ring_position); + return; + } + + if (fsm->take_time) { + fsm->take_time = 0; + fsm->jiffies_start = datagram->jiffies_sent; + } + + slave->current_state = EC_READ_U8(datagram->data); + + if (slave->current_state == fsm->requested_state) { + // state has been set successfully + fsm->state = ec_fsm_change_end; + return; + } + + if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { + // state change error + EC_ERR("Failed to set state 0x%02X - Slave %i refused state change" + " (code 0x%02X)!\n", fsm->requested_state, slave->ring_position, + slave->current_state); + // fetch AL status error code + ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); + ec_master_queue_datagram(fsm->slave->master, datagram); + fsm->state = ec_fsm_change_code; + return; + } + + if (datagram->jiffies_received + - fsm->jiffies_start >= 100 * HZ / 1000) { // 100ms + // timeout while checking + fsm->state = ec_fsm_change_error; + EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", + fsm->requested_state, slave->ring_position); + return; + } + + // still old state: check again + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->slave->master, datagram); +} + +/*****************************************************************************/ + +/** + Application layer status messages. +*/ + +const ec_code_msg_t al_status_messages[] = { + {0x0001, "Unspecified error"}, + {0x0011, "Invalud requested state change"}, + {0x0012, "Unknown requested state"}, + {0x0013, "Bootstrap not supported"}, + {0x0014, "No valid firmware"}, + {0x0015, "Invalid mailbox configuration"}, + {0x0016, "Invalid mailbox configuration"}, + {0x0017, "Invalid sync manager configuration"}, + {0x0018, "No valid inputs available"}, + {0x0019, "No valid outputs"}, + {0x001A, "Synchronisation error"}, + {0x001B, "Sync manager watchdog"}, + {0x001C, "Invalid sync manager types"}, + {0x001D, "Invalid output configuration"}, + {0x001E, "Invalid input configuration"}, + {0x001F, "Invalid watchdog configuration"}, + {0x0020, "Slave needs cold start"}, + {0x0021, "Slave needs INIT"}, + {0x0022, "Slave needs PREOP"}, + {0x0023, "Slave needs SAVEOP"}, + {0x0030, "Invalid DC SYNCH configuration"}, + {0x0031, "Invalid DC latch configuration"}, + {0x0032, "PLL error"}, + {0x0033, "Invalid DC IO error"}, + {0x0034, "Invalid DC timeout error"}, + {0x0042, "MBOX EOE"}, + {0x0043, "MBOX COE"}, + {0x0044, "MBOX FOE"}, + {0x0045, "MBOX SOE"}, + {0x004F, "MBOX VOE"}, + {} +}; + +/*****************************************************************************/ + +/** + Change state: CODE. +*/ + +void ec_fsm_change_code(ec_fsm_change_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + uint32_t code; + const ec_code_msg_t *al_msg; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + EC_WARN("Reception of AL status code datagram failed.\n"); + } + else { + if ((code = EC_READ_U16(datagram->data))) { + for (al_msg = al_status_messages; al_msg->code; al_msg++) { + if (al_msg->code != code) continue; + EC_ERR("AL status message 0x%04X: \"%s\".\n", + al_msg->code, al_msg->message); + break; + } + if (!al_msg->code) + EC_ERR("Unknown AL status code 0x%04X.\n", code); + } + } + + // acknowledge "old" slave state + ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); + EC_WRITE_U16(datagram->data, slave->current_state); + ec_master_queue_datagram(fsm->slave->master, datagram); + fsm->state = ec_fsm_change_ack; +} + +/*****************************************************************************/ + +/** + Change state: ACK. +*/ + +void ec_fsm_change_ack(ec_fsm_change_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->state = ec_fsm_change_error; + EC_ERR("Reception of state ack datagram failed.\n"); + return; + } + + fsm->take_time = 1; + + // read new AL status + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->slave->master, datagram); + fsm->state = ec_fsm_change_check_ack; +} + +/*****************************************************************************/ + +/** + Change state: CHECK ACK. +*/ + +void ec_fsm_change_check_ack(ec_fsm_change_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_DATAGRAM_RECEIVED + || datagram->working_counter != 1) { + fsm->state = ec_fsm_change_error; + EC_ERR("Reception of state ack check datagram failed.\n"); + return; + } + + if (fsm->take_time) { + fsm->take_time = 0; + fsm->jiffies_start = datagram->jiffies_sent; + } + + slave->current_state = EC_READ_U8(datagram->data); + + if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) { + fsm->state = ec_fsm_change_error; + EC_INFO("Acknowledged state 0x%02X on slave %i.\n", + slave->current_state, slave->ring_position); + return; + } + + if (datagram->jiffies_received + - fsm->jiffies_start >= 100 * HZ / 1000) { // 100ms + // timeout while checking + slave->current_state = EC_SLAVE_STATE_UNKNOWN; + fsm->state = ec_fsm_change_error; + EC_ERR("Timeout while acknowledging state 0x%02X on slave %i.\n", + fsm->requested_state, slave->ring_position); + return; + } + + // reread new AL status + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->slave->master, datagram); +} + +/*****************************************************************************/ + +/** + State: ERROR. +*/ + +void ec_fsm_change_error(ec_fsm_change_t *fsm /**< finite state machine */) +{ +} + +/*****************************************************************************/ + +/** + State: END. +*/ + +void ec_fsm_change_end(ec_fsm_change_t *fsm /**< finite state machine */) +{ +} + +/*****************************************************************************/ diff -r 100f51f28cf2 -r 0180d8277311 master/fsm_change.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_change.h Mon Oct 23 13:45:09 2006 +0000 @@ -0,0 +1,81 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it + * and/or modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * The IgH EtherCAT Master is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * The right to use EtherCAT Technology is granted and comes free of + * charge under condition of compatibility of product made by + * Licensee. People intending to distribute/sell products based on the + * code, have to sign an agreement to guarantee that products using + * software based on IgH EtherCAT master stay compatible with the actual + * EtherCAT specification (which are released themselves as an open + * standard) as the (only) precondition to have the right to use EtherCAT + * Technology, IP and trade marks. + * + *****************************************************************************/ + +/** + \file + EtherCAT state change FSM. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_CHANGE__ +#define __EC_FSM_CHANGE__ + +#include "globals.h" +#include "../include/ecrt.h" +#include "datagram.h" +#include "slave.h" + +/*****************************************************************************/ + +typedef struct ec_fsm_change ec_fsm_change_t; /**< \see ec_fsm_change */ + +/** + EtherCAT state change FSM. +*/ + +struct ec_fsm_change +{ + ec_slave_t *slave; /**< slave the FSM runs on */ + ec_datagram_t *datagram; /**< datagram used in the state machine */ + + void (*state)(ec_fsm_change_t *); /**< slave state change state function */ + ec_slave_state_t requested_state; /**< input: state */ + unsigned long jiffies_start; /**< change timer */ + uint8_t take_time; /**< take sending timestamp */ +}; + +/*****************************************************************************/ + +void ec_fsm_change_init(ec_fsm_change_t *, ec_datagram_t *); +void ec_fsm_change_clear(ec_fsm_change_t *); + +void ec_fsm_change(ec_fsm_change_t *, ec_slave_t *, ec_slave_state_t); + +void ec_fsm_change_exec(ec_fsm_change_t *); +int ec_fsm_change_running(ec_fsm_change_t *); +int ec_fsm_change_success(ec_fsm_change_t *); + +/*****************************************************************************/ + +#endif