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