bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ethercat/master/fsm_change.c

552 lines
17 KiB
C
Raw Blame History

/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006-2008 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 version 2, as
* published by the Free Software Foundation.
*
* 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
*
* Using the EtherCAT technology and brand is permitted in compliance with
* the industrial property and similar rights of Beckhoff Automation GmbH.
*
*****************************************************************************/
/**
\file
EtherCAT state change FSM.
*/
/*****************************************************************************/
#include "globals.h"
#include "master.h"
#include "fsm_change.h"
/*****************************************************************************/
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 *);
/*****************************************************************************/
/**
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;
fsm->spontaneous_change = 0;
}
/*****************************************************************************/
/**
Destructor.
*/
void ec_fsm_change_clear(ec_fsm_change_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/
/**
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_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;
}
/*****************************************************************************/
/**
Executes the current state of the state machine.
\return false, if the state machine has terminated
*/
int ec_fsm_change_exec(ec_fsm_change_t *fsm /**< finite state machine */)
{
fsm->state(fsm);
return fsm->state != ec_fsm_change_state_end
&& fsm->state != ec_fsm_change_state_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_state_end;
}
/******************************************************************************
* state change state machine
*****************************************************************************/
/**
Change state: START.
*/
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_fpwr(datagram, slave->station_address, 0x0120, 2);
EC_WRITE_U16(datagram->data, fsm->requested_state);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_check;
}
/*****************************************************************************/
/**
Change state: CHECK.
*/
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_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to receive state datagram from slave %u"
" (datagram state %u)!\n",
fsm->slave->ring_position, datagram->state);
return;
}
if (fsm->take_time) {
fsm->take_time = 0;
fsm->jiffies_start = datagram->jiffies_sent;
}
if (datagram->working_counter == 0) {
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_state_error;
EC_ERR("Failed to set state %s on slave %u: ",
state_str, fsm->slave->ring_position);
ec_datagram_print_wc_error(datagram);
return;
}
// repeat writing new state to slave
ec_datagram_fpwr(datagram, slave->station_address, 0x0120, 2);
EC_WRITE_U16(datagram->data, fsm->requested_state);
fsm->retries = EC_FSM_RETRIES;
return;
}
if (unlikely(datagram->working_counter > 1)) {
char state_str[EC_STATE_STRING_SIZE];
ec_state_string(fsm->requested_state, state_str);
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to set state %s on slave %u: ",
state_str, fsm->slave->ring_position);
ec_datagram_print_wc_error(datagram);
return;
}
fsm->take_time = 1;
// read AL status from slave
ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->spontaneous_change = 0;
fsm->state = ec_fsm_change_state_status;
}
/*****************************************************************************/
/**
Change state: STATUS.
*/
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_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to receive state checking datagram from slave %u"
" (datagram state %u).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
char req_state[EC_STATE_STRING_SIZE];
ec_state_string(fsm->requested_state, req_state);
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to check state %s on slave %u: ",
req_state, slave->ring_position);
ec_datagram_print_wc_error(datagram);
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_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];
ec_state_string(slave->current_state, cur_state);
if ((slave->current_state & 0x0F) != (fsm->old_state & 0x0F)) {
// Slave spontaneously changed its state just before the new state
// was written. Accept current state as old state and wait for
// state change
fsm->spontaneous_change = 1;
fsm->old_state = slave->current_state;
EC_WARN("Slave %u changed to %s in the meantime.\n",
slave->ring_position, cur_state);
goto check_again;
}
// state change error
slave->error_flag = 1;
ec_state_string(fsm->requested_state, req_state);
EC_ERR("Failed to set %s state, slave %u refused state change (%s).\n",
req_state, slave->ring_position, cur_state);
// fetch AL status error code
ec_datagram_fprd(datagram, slave->station_address, 0x0134, 2);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_code;
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_state_error;
EC_ERR("Timeout while setting state %s on slave %u.\n",
state_str, slave->ring_position);
return;
}
check_again:
// no timeout yet. check again
ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
}
/*****************************************************************************/
/**
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 SAFEOP"},
{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_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;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to receive AL status code datagram from slave %u"
" (datagram state %u).\n",
fsm->slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
EC_WARN("Reception of AL status code datagram failed: ");
ec_datagram_print_wc_error(datagram);
} else {
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_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_fpwr(datagram, slave->station_address, 0x0120, 2);
EC_WRITE_U16(datagram->data, slave->current_state);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_ack;
}
/*****************************************************************************/
/**
Change state: ACK.
*/
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_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to receive state ack datagram for slave %u"
" (datagram state %u).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Reception of state ack datagram failed on slave %u: ",
slave->ring_position);
ec_datagram_print_wc_error(datagram);
return;
}
fsm->take_time = 1;
// read new AL status
ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_change_state_check_ack;
}
/*****************************************************************************/
/**
Change state: CHECK ACK.
*/
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_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Failed to receive state ack check datagram from slave %u"
" (datagram state %u).\n",
slave->ring_position, datagram->state);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_change_state_error;
EC_ERR("Reception of state ack check datagram failed on slave %u: ",
slave->ring_position);
ec_datagram_print_wc_error(datagram);
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)) {
char state_str[EC_STATE_STRING_SIZE];
ec_state_string(slave->current_state, state_str);
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 %u.\n",
state_str, slave->ring_position);
return;
}
if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s
// timeout while checking
char state_str[EC_STATE_STRING_SIZE];
ec_state_string(slave->current_state, state_str);
fsm->state = ec_fsm_change_state_error;
EC_ERR("Timeout while acknowledging state %s on slave %u.\n",
state_str, slave->ring_position);
return;
}
// reread new AL status
ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
}
/*****************************************************************************/
/**
State: ERROR.
*/
void ec_fsm_change_state_error(ec_fsm_change_t *fsm
/**< finite state machine */)
{
}
/*****************************************************************************/
/**
State: END.
*/
void ec_fsm_change_state_end(ec_fsm_change_t *fsm
/**< finite state machine */)
{
}
/*****************************************************************************/
Reference in New Issue View Git Blame Copy Permalink