# HG changeset patch # User Florian Pose # Date 1161947707 0 # Node ID 25cc4a3b9e0adc63039cf769807853b00966ab42 # Parent 2ecaa53c6291101fa1115e434db2012bbf25f6a1 State acknowledgement in master state machine. diff -r 2ecaa53c6291 -r 25cc4a3b9e0a examples/mini/mini.c --- a/examples/mini/mini.c Fri Oct 27 10:41:02 2006 +0000 +++ b/examples/mini/mini.c Fri Oct 27 11:15:07 2006 +0000 @@ -52,7 +52,7 @@ spinlock_t master_lock = SPIN_LOCK_UNLOCKED; // data fields -#if 0 +#if 1 void *r_inputs; void *r_outputs; #endif @@ -81,7 +81,7 @@ // process data //k_pos = EC_READ_U32(r_ssi); - //EC_WRITE_U8(r_outputs + 2, einaus ? 0xFF : 0x00); + EC_WRITE_U8(r_outputs + 2, einaus ? 0xFF : 0x00); // send ecrt_master_run(master); @@ -121,7 +121,9 @@ int __init init_mini_module(void) { - // ec_slave_t *slave; +#if 0 + ec_slave_t *slave; +#endif printk(KERN_INFO "=== Starting Minimal EtherCAT environment... ===\n"); @@ -147,7 +149,7 @@ } #endif -#if 0 +#if 1 if (!ecrt_domain_register_pdo_range(domain1, "0", Beckhoff_BK1120, EC_DIR_OUTPUT, 0, 4, &r_outputs)) { printk(KERN_ERR "PDO registration failed!\n"); diff -r 2ecaa53c6291 -r 25cc4a3b9e0a master/fsm.c --- a/master/fsm.c Fri Oct 27 10:41:02 2006 +0000 +++ b/master/fsm.c Fri Oct 27 11:15:07 2006 +0000 @@ -48,6 +48,7 @@ void ec_fsm_master_start(ec_fsm_t *); void ec_fsm_master_broadcast(ec_fsm_t *); void ec_fsm_master_read_states(ec_fsm_t *); +void ec_fsm_master_acknowledge(ec_fsm_t *); void ec_fsm_master_validate_vendor(ec_fsm_t *); void ec_fsm_master_validate_product(ec_fsm_t *); void ec_fsm_master_rewrite_addresses(ec_fsm_t *); @@ -321,7 +322,7 @@ fsm->slave = slave; fsm->slave_state = ec_fsm_slaveconf_init; - ec_fsm_change(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); + ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); fsm->master_state = ec_fsm_master_configure_slave; fsm->master_state(fsm); // execute immediately return; @@ -507,6 +508,37 @@ slave->current_state = new_state; } + // check, if new slave state has to be acknowledged + if (slave->current_state & EC_SLAVE_STATE_ACK_ERR && !slave->error_flag) { + ec_fsm_change_ack(&fsm->fsm_change, slave); + ec_fsm_change_exec(&fsm->fsm_change); + fsm->master_state = ec_fsm_master_acknowledge; + return; + } + + ec_fsm_master_action_next_slave_state(fsm); +} + +/*****************************************************************************/ + +/** + Master state: ACKNOWLEDGE +*/ + +void ec_fsm_master_acknowledge(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_slave_t *slave = fsm->slave; + + if (ec_fsm_change_exec(&fsm->fsm_change)) return; + + if (!ec_fsm_change_success(&fsm->fsm_change)) { + fsm->slave->error_flag = 1; + EC_ERR("Failed to acknowledge state change on slave %i.\n", + slave->ring_position); + fsm->master_state = ec_fsm_master_error; + return; + } + ec_fsm_master_action_next_slave_state(fsm); } @@ -903,8 +935,10 @@ slave->current_state = EC_READ_U8(datagram->data); if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { - EC_WARN("Slave %i has state error bit set (0x%02X)!\n", - slave->ring_position, slave->current_state); + char state_str[EC_STATE_STRING_SIZE]; + ec_state_string(slave->current_state, state_str); + EC_WARN("Slave %i has state error bit set (%s)!\n", + slave->ring_position, state_str); } // read base data @@ -1200,7 +1234,7 @@ if (!slave->base_sync_count) { // no sync managers fsm->slave_state = ec_fsm_slaveconf_preop; - ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_PREOP); + ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_PREOP); ec_fsm_change_exec(&fsm->fsm_change); // execute immediately return; } @@ -1276,7 +1310,7 @@ } fsm->slave_state = ec_fsm_slaveconf_preop; - ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_PREOP); + ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_PREOP); ec_fsm_change_exec(&fsm->fsm_change); // execute immediately } @@ -1320,7 +1354,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; - ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_SAVEOP); + ec_fsm_change_start(&fsm->fsm_change, slave, + EC_SLAVE_STATE_SAVEOP); ec_fsm_change_exec(&fsm->fsm_change); // execute immediately return; } @@ -1370,7 +1405,7 @@ if (list_empty(&slave->sdo_confs)) { // skip SDO configuration // set state to SAVEOP fsm->slave_state = ec_fsm_slaveconf_saveop; - ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_SAVEOP); + ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_SAVEOP); ec_fsm_change_exec(&fsm->fsm_change); // execute immediately return; } @@ -1411,7 +1446,7 @@ // set state to SAVEOP fsm->slave_state = ec_fsm_slaveconf_saveop; - ec_fsm_change(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAVEOP); + ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAVEOP); ec_fsm_change_exec(&fsm->fsm_change); // execute immediately } @@ -1451,7 +1486,7 @@ // set state to OP fsm->slave_state = ec_fsm_slaveconf_op; - ec_fsm_change(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP); + ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP); ec_fsm_change_exec(&fsm->fsm_change); // execute immediately } diff -r 2ecaa53c6291 -r 25cc4a3b9e0a master/fsm_change.c --- a/master/fsm_change.c Fri Oct 27 10:41:02 2006 +0000 +++ b/master/fsm_change.c Fri Oct 27 11:15:07 2006 +0000 @@ -44,14 +44,15 @@ /*****************************************************************************/ -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 *); +void ec_fsm_change_state_start(ec_fsm_change_t *); +void ec_fsm_change_state_check(ec_fsm_change_t *); +void ec_fsm_change_state_status(ec_fsm_change_t *); +void ec_fsm_change_state_code(ec_fsm_change_t *); +void ec_fsm_change_state_start_ack(ec_fsm_change_t *); +void ec_fsm_change_state_ack(ec_fsm_change_t *); +void ec_fsm_change_state_check_ack(ec_fsm_change_t *); +void ec_fsm_change_state_end(ec_fsm_change_t *); +void ec_fsm_change_state_error(ec_fsm_change_t *); /*****************************************************************************/ @@ -80,17 +81,34 @@ /*****************************************************************************/ /** - 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 */ - ) -{ + Starts the change state machine. +*/ + +void ec_fsm_change_start(ec_fsm_change_t *fsm, /**< finite state machine */ + ec_slave_t *slave, /**< EtherCAT slave */ + ec_slave_state_t state /**< requested state */ + ) +{ + fsm->mode = EC_FSM_CHANGE_MODE_FULL; fsm->slave = slave; fsm->requested_state = state; - fsm->state = ec_fsm_change_start; + fsm->state = ec_fsm_change_state_start; +} + +/*****************************************************************************/ + +/** + Starts the change state machine to only acknowlegde a slave's state. +*/ + +void ec_fsm_change_ack(ec_fsm_change_t *fsm, /**< finite state machine */ + ec_slave_t *slave /**< EtherCAT slave */ + ) +{ + fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY; + fsm->slave = slave; + fsm->requested_state = EC_SLAVE_STATE_UNKNOWN; + fsm->state = ec_fsm_change_state_start_ack; } /*****************************************************************************/ @@ -104,8 +122,8 @@ { fsm->state(fsm); - return fsm->state != ec_fsm_change_end - && fsm->state != ec_fsm_change_error; + return fsm->state != ec_fsm_change_state_end + && fsm->state != ec_fsm_change_state_error; } /*****************************************************************************/ @@ -117,7 +135,7 @@ int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */) { - return fsm->state == ec_fsm_change_end; + return fsm->state == ec_fsm_change_state_end; } /****************************************************************************** @@ -128,18 +146,20 @@ Change state: START. */ -void ec_fsm_change_start(ec_fsm_change_t *fsm /**< finite state machine */) +void ec_fsm_change_state_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; + fsm->old_state = fsm->slave->current_state; // 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; + fsm->state = ec_fsm_change_state_check; } /*****************************************************************************/ @@ -148,13 +168,14 @@ Change state: CHECK. */ -void ec_fsm_change_check(ec_fsm_change_t *fsm /**< finite state machine */) +void ec_fsm_change_state_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; + fsm->state = ec_fsm_change_state_error; EC_ERR("Failed to send state datagram to slave %i!\n", fsm->slave->ring_position); return; @@ -169,7 +190,7 @@ if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) { char state_str[EC_STATE_STRING_SIZE]; ec_state_string(fsm->requested_state, state_str); - fsm->state = ec_fsm_change_error; + fsm->state = ec_fsm_change_state_error; EC_ERR("Failed to set state %s on slave %i: Slave did not" " respond.\n", state_str, fsm->slave->ring_position); return; @@ -187,7 +208,7 @@ // 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; + fsm->state = ec_fsm_change_state_status; } /*****************************************************************************/ @@ -196,14 +217,15 @@ Change state: STATUS. */ -void ec_fsm_change_status(ec_fsm_change_t *fsm /**< finite state machine */) +void ec_fsm_change_state_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; + fsm->state = ec_fsm_change_state_error; EC_ERR("Failed to check state 0x%02X on slave %i.\n", fsm->requested_state, slave->ring_position); return; @@ -218,35 +240,49 @@ 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 + fsm->state = ec_fsm_change_state_end; + return; + } + + if (slave->current_state != fsm->old_state) { // state changed char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE]; + + slave->error_flag = 1; ec_state_string(fsm->requested_state, req_state); ec_state_string(slave->current_state, cur_state); - EC_ERR("Failed to set %s state, slave %i refused state change (%s).\n", - req_state, slave->ring_position, cur_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 (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { + // state change error + EC_ERR("Failed to set %s state," + " slave %i refused state change (%s).\n", + req_state, slave->ring_position, cur_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_state_code; + return; + } + + // state change to unrequested state + EC_ERR("Slave %i changed to unrequested state %s!\n", + slave->ring_position, cur_state); + fsm->state = ec_fsm_change_state_error; + return; + } + + // still old state if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s // timeout while checking char state_str[EC_STATE_STRING_SIZE]; ec_state_string(fsm->requested_state, state_str); - fsm->state = ec_fsm_change_error; + fsm->state = ec_fsm_change_state_error; EC_ERR("Timeout while setting state %s on slave %i.\n", state_str, slave->ring_position); return; } - // still old state: check again + // check again ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); ec_master_queue_datagram(fsm->slave->master, datagram); } @@ -297,10 +333,10 @@ 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; +void ec_fsm_change_state_code(ec_fsm_change_t *fsm + /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; uint32_t code; const ec_code_msg_t *al_msg; @@ -322,10 +358,25 @@ } // acknowledge "old" slave state + ec_fsm_change_state_start_ack(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + Change state: START ACK. +*/ + +void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm + /**< finite state machine */) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = fsm->datagram; + 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; + fsm->state = ec_fsm_change_state_ack; } /*****************************************************************************/ @@ -334,14 +385,14 @@ Change state: ACK. */ -void ec_fsm_change_ack(ec_fsm_change_t *fsm /**< finite state machine */) +void ec_fsm_change_state_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; + fsm->state = ec_fsm_change_state_error; EC_ERR("Reception of state ack datagram failed.\n"); return; } @@ -351,7 +402,7 @@ // 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; + fsm->state = ec_fsm_change_state_check_ack; } /*****************************************************************************/ @@ -360,14 +411,15 @@ Change state: CHECK ACK. */ -void ec_fsm_change_check_ack(ec_fsm_change_t *fsm /**< finite state machine */) +void ec_fsm_change_state_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; + fsm->state = ec_fsm_change_state_error; EC_ERR("Reception of state ack check datagram failed.\n"); return; } @@ -382,7 +434,12 @@ if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) { char state_str[EC_STATE_STRING_SIZE]; ec_state_string(slave->current_state, state_str); - fsm->state = ec_fsm_change_error; + if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) { + fsm->state = ec_fsm_change_state_error; + } + else { // EC_FSM_CHANGE_MODE_ACK_ONLY + fsm->state = ec_fsm_change_state_end; + } EC_INFO("Acknowledged state %s on slave %i.\n", state_str, slave->ring_position); return; @@ -392,7 +449,7 @@ // timeout while checking char state_str[EC_STATE_STRING_SIZE]; ec_state_string(slave->current_state, state_str); - fsm->state = ec_fsm_change_error; + fsm->state = ec_fsm_change_state_error; EC_ERR("Timeout while acknowledging state %s on slave %i.\n", state_str, slave->ring_position); return; @@ -409,7 +466,8 @@ State: ERROR. */ -void ec_fsm_change_error(ec_fsm_change_t *fsm /**< finite state machine */) +void ec_fsm_change_state_error(ec_fsm_change_t *fsm + /**< finite state machine */) { } @@ -419,8 +477,9 @@ State: END. */ -void ec_fsm_change_end(ec_fsm_change_t *fsm /**< finite state machine */) -{ -} - -/*****************************************************************************/ +void ec_fsm_change_state_end(ec_fsm_change_t *fsm + /**< finite state machine */) +{ +} + +/*****************************************************************************/ diff -r 2ecaa53c6291 -r 25cc4a3b9e0a master/fsm_change.h --- a/master/fsm_change.h Fri Oct 27 10:41:02 2006 +0000 +++ b/master/fsm_change.h Fri Oct 27 11:15:07 2006 +0000 @@ -48,6 +48,18 @@ /*****************************************************************************/ +/** + Mode of the change state machine. +*/ + +typedef enum { + EC_FSM_CHANGE_MODE_FULL, /**< full state change */ + EC_FSM_CHANGE_MODE_ACK_ONLY /**< only state acknowledgement */ +} +ec_fsm_change_mode_t; + +/*****************************************************************************/ + typedef struct ec_fsm_change ec_fsm_change_t; /**< \see ec_fsm_change */ /** @@ -60,7 +72,9 @@ ec_datagram_t *datagram; /**< datagram used in the state machine */ void (*state)(ec_fsm_change_t *); /**< slave state change state function */ + ec_fsm_change_mode_t mode; /**< full state change, or ack only. */ ec_slave_state_t requested_state; /**< input: state */ + ec_slave_state_t old_state; /**< prior slave state */ unsigned long jiffies_start; /**< change timer */ uint8_t take_time; /**< take sending timestamp */ }; @@ -70,7 +84,8 @@ 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_start(ec_fsm_change_t *, ec_slave_t *, ec_slave_state_t); +void ec_fsm_change_ack(ec_fsm_change_t *, ec_slave_t *); int ec_fsm_change_exec(ec_fsm_change_t *); int ec_fsm_change_success(ec_fsm_change_t *);