/******************************************************************************
*
* $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 finite state machines.
*/
/*****************************************************************************/
#include "globals.h"
#include "fsm.h"
#include "master.h"
#include "mailbox.h"
/*****************************************************************************/
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 *);
void ec_fsm_master_configure_slave(ec_fsm_t *);
void ec_fsm_master_scan_slaves(ec_fsm_t *);
void ec_fsm_master_write_eeprom(ec_fsm_t *);
void ec_fsm_master_sdodict(ec_fsm_t *);
void ec_fsm_master_sdo_request(ec_fsm_t *);
void ec_fsm_master_end(ec_fsm_t *);
void ec_fsm_master_error(ec_fsm_t *);
void ec_fsm_slavescan_start(ec_fsm_t *);
void ec_fsm_slavescan_address(ec_fsm_t *);
void ec_fsm_slavescan_state(ec_fsm_t *);
void ec_fsm_slavescan_base(ec_fsm_t *);
void ec_fsm_slavescan_datalink(ec_fsm_t *);
void ec_fsm_slavescan_eeprom_size(ec_fsm_t *);
void ec_fsm_slavescan_eeprom_data(ec_fsm_t *);
void ec_fsm_slaveconf_state_start(ec_fsm_t *);
void ec_fsm_slaveconf_state_init(ec_fsm_t *);
void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *);
void ec_fsm_slaveconf_state_sync(ec_fsm_t *);
void ec_fsm_slaveconf_state_preop(ec_fsm_t *);
void ec_fsm_slaveconf_state_sync2(ec_fsm_t *);
void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *);
void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *);
void ec_fsm_slaveconf_state_saveop(ec_fsm_t *);
void ec_fsm_slaveconf_state_op(ec_fsm_t *);
void ec_fsm_slaveconf_enter_sync(ec_fsm_t *);
void ec_fsm_slaveconf_enter_preop(ec_fsm_t *);
void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *);
void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *);
void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *);
void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *);
void ec_fsm_slave_state_end(ec_fsm_t *);
void ec_fsm_slave_state_error(ec_fsm_t *);
/*****************************************************************************/
/**
Constructor.
*/
int ec_fsm_init(ec_fsm_t *fsm, /**< finite state machine */
ec_master_t *master /**< EtherCAT master */
)
{
fsm->master = master;
fsm->master_state = ec_fsm_master_start;
fsm->master_slaves_responding = 0;
fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;
fsm->master_validation = 0;
ec_datagram_init(&fsm->datagram);
if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) {
EC_ERR("Failed to allocate FSM datagram.\n");
return -1;
}
// init sub-state-machines
ec_fsm_sii_init(&fsm->fsm_sii, &fsm->datagram);
ec_fsm_change_init(&fsm->fsm_change, &fsm->datagram);
ec_fsm_coe_init(&fsm->fsm_coe, &fsm->datagram);
return 0;
}
/*****************************************************************************/
/**
Destructor.
*/
void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)
{
// clear sub-state machines
ec_fsm_sii_clear(&fsm->fsm_sii);
ec_fsm_change_clear(&fsm->fsm_change);
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_datagram_clear(&fsm->datagram);
}
/*****************************************************************************/
/**
Executes the current state of the state machine.
If the state machine's datagram is not sent or received yet, the execution
of the state machine is delayed to the next cycle.
\return false, if state machine has terminated
*/
int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)
{
if (fsm->datagram.state == EC_DATAGRAM_SENT
|| fsm->datagram.state == EC_DATAGRAM_QUEUED) {
// datagram was not sent or received yet.
return ec_fsm_running(fsm);
}
fsm->master_state(fsm);
return ec_fsm_running(fsm);
}
/*****************************************************************************/
/**
\return false, if state machine has terminated
*/
int ec_fsm_running(ec_fsm_t *fsm /**< finite state machine */)
{
return fsm->master_state != ec_fsm_master_end
&& fsm->master_state != ec_fsm_master_error;
}
/*****************************************************************************/
/**
\return true, if the master state machine terminated gracefully
*/
int ec_fsm_success(ec_fsm_t *fsm /**< finite state machine */)
{
return fsm->master_state == ec_fsm_master_end;
}
/******************************************************************************
* operation/idle state machine
*****************************************************************************/
/**
Master state: START.
Starts with getting slave count and slave states.
*/
void ec_fsm_master_start(ec_fsm_t *fsm)
{
ec_datagram_brd(&fsm->datagram, 0x0130, 2);
ec_master_queue_datagram(fsm->master, &fsm->datagram);
fsm->master_state = ec_fsm_master_broadcast;
}
/*****************************************************************************/
/**
Master state: BROADCAST.
Processes the broadcast read slave count and slaves states.
*/
void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
unsigned int topology_change, states_change, i;
ec_slave_t *slave;
ec_master_t *master = fsm->master;
if (datagram->state == EC_DATAGRAM_TIMED_OUT) {
// always retry
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) { // EC_DATAGRAM_ERROR
// link is down
fsm->master_slaves_responding = 0;
list_for_each_entry(slave, &master->slaves, list) {
slave->online = 0;
}
fsm->master_state = ec_fsm_master_error;
return;
}
topology_change = (datagram->working_counter !=
fsm->master_slaves_responding);
states_change = (EC_READ_U8(datagram->data) != fsm->master_slave_states);
fsm->master_slave_states = EC_READ_U8(datagram->data);
fsm->master_slaves_responding = datagram->working_counter;
if (topology_change) {
EC_INFO("%i slave%s responding.\n",
fsm->master_slaves_responding,
fsm->master_slaves_responding == 1 ? "" : "s");
if (master->mode == EC_MASTER_MODE_OPERATION) {
if (fsm->master_slaves_responding == master->slave_count) {
fsm->master_validation = 1; // start validation later
}
else {
EC_WARN("Invalid slave count. Bus in tainted state.\n");
}
}
}
if (states_change) {
char states[EC_STATE_STRING_SIZE];
ec_state_string(fsm->master_slave_states, states);
EC_INFO("Slave states: %s.\n", states);
}
// topology change in idle mode: clear all slaves and scan the bus
if (topology_change && master->mode == EC_MASTER_MODE_IDLE) {
ec_master_eoe_stop(master);
ec_master_destroy_slaves(master);
master->slave_count = datagram->working_counter;
if (!master->slave_count) {
// no slaves present -> finish state machine.
fsm->master_state = ec_fsm_master_end;
return;
}
// init slaves
for (i = 0; i < master->slave_count; i++) {
if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),
GFP_ATOMIC))) {
EC_ERR("Failed to allocate slave %i!\n", i);
ec_master_destroy_slaves(master);
fsm->master_state = ec_fsm_master_error;
return;
}
if (ec_slave_init(slave, master, i, i + 1)) {
// freeing of "slave" already done
ec_master_destroy_slaves(master);
fsm->master_state = ec_fsm_master_error;
return;
}
list_add_tail(&slave->list, &master->slaves);
}
EC_INFO("Scanning bus.\n");
// begin scanning of slaves
fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
fsm->slave_state = ec_fsm_slavescan_start;
fsm->master_state = ec_fsm_master_scan_slaves;
fsm->master_state(fsm); // execute immediately
return;
}
// fetch state from each slave
fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2);
ec_master_queue_datagram(master, &fsm->datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->master_state = ec_fsm_master_read_states;
}
/*****************************************************************************/
/**
Master action: PROC_STATES.
Processes the slave states.
*/
void ec_fsm_master_action_process_states(ec_fsm_t *fsm
/**< finite state machine */
)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave;
char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];
// check if any slaves are not in the state, they're supposed to be
list_for_each_entry(slave, &master->slaves, list) {
if (slave->error_flag
|| !slave->online
|| slave->requested_state == EC_SLAVE_STATE_UNKNOWN
|| (slave->current_state == slave->requested_state
&& slave->configured)) continue;
if (master->debug_level) {
ec_state_string(slave->current_state, old_state);
if (slave->current_state != slave->requested_state) {
ec_state_string(slave->requested_state, new_state);
EC_DBG("Changing state of slave %i (%s -> %s).\n",
slave->ring_position, old_state, new_state);
}
else if (!slave->configured) {
EC_DBG("Reconfiguring slave %i (%s).\n",
slave->ring_position, old_state);
}
}
fsm->master_state = ec_fsm_master_configure_slave;
fsm->slave = slave;
fsm->slave_state = ec_fsm_slaveconf_state_start;
fsm->slave_state(fsm); // execute immediately
return;
}
// Check, if EoE processing has to be started
ec_master_eoe_start(master);
if (master->mode == EC_MASTER_MODE_IDLE) {
// Check for a pending SDO request
if (master->sdo_seq_master != master->sdo_seq_user) {
if (master->debug_level)
EC_DBG("Processing SDO request...\n");
slave = master->sdo_request->sdo->slave;
if (slave->current_state == EC_SLAVE_STATE_INIT
|| !slave->online) {
EC_ERR("Failed to process SDO request, slave %i not ready.\n",
slave->ring_position);
master->sdo_request->return_code = -1;
master->sdo_seq_master++;
}
else {
// start uploading SDO
fsm->slave = slave;
fsm->master_state = ec_fsm_master_sdo_request;
fsm->sdo_request = master->sdo_request;
ec_fsm_coe_upload(&fsm->fsm_coe, slave, fsm->sdo_request);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
return;
}
}
// check, if slaves have an SDO dictionary to read out.
list_for_each_entry(slave, &master->slaves, list) {
if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)
|| slave->sdo_dictionary_fetched
|| slave->current_state == EC_SLAVE_STATE_INIT
|| jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ
|| !slave->online
|| slave->error_flag) continue;
if (master->debug_level) {
EC_DBG("Fetching SDO dictionary from slave %i.\n",
slave->ring_position);
}
slave->sdo_dictionary_fetched = 1;
// start fetching SDO dictionary
fsm->slave = slave;
fsm->master_state = ec_fsm_master_sdodict;
ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
return;
}
// check for pending EEPROM write operations.
list_for_each_entry(slave, &master->slaves, list) {
if (!slave->new_eeprom_data) continue;
if (!slave->online || slave->error_flag) {
kfree(slave->new_eeprom_data);
slave->new_eeprom_data = NULL;
EC_ERR("Discarding EEPROM data, slave %i not ready.\n",
slave->ring_position);
continue;
}
// found pending EEPROM write operation. execute it!
EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position);
fsm->slave = slave;
fsm->sii_offset = 0x0000;
ec_fsm_sii_write(&fsm->fsm_sii, slave, fsm->sii_offset,
slave->new_eeprom_data, EC_FSM_SII_NODE);
fsm->master_state = ec_fsm_master_write_eeprom;
fsm->master_state(fsm); // execute immediately
return;
}
}
fsm->master_state = ec_fsm_master_end;
}
/*****************************************************************************/
/**
Master action: Get state of next slave.
*/
void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm
/**< finite state machine */)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave = fsm->slave;
// is there another slave to query?
if (slave->list.next != &master->slaves) {
// process next slave
fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address,
0x0130, 2);
ec_master_queue_datagram(master, &fsm->datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->master_state = ec_fsm_master_read_states;
return;
}
// all slave states read
// check, if a bus validation has to be done
if (fsm->master_validation) {
fsm->master_validation = 0;
list_for_each_entry(slave, &master->slaves, list) {
if (slave->online) continue;
// At least one slave is offline. validate!
EC_INFO("Validating bus.\n");
fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
fsm->master_state = ec_fsm_master_validate_vendor;
ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x0008, EC_FSM_SII_POSITION);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute immediately
return;
}
}
ec_fsm_master_action_process_states(fsm);
}
/*****************************************************************************/
/**
Master state: READ STATES.
Fetches the AL- and online state of a slave.
*/
void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = &fsm->datagram;
uint8_t new_state;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive AL state datagram for slave %i!\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
// did the slave not respond to its station address?
if (datagram->working_counter != 1) {
if (slave->online) {
slave->online = 0;
if (slave->master->debug_level)
EC_DBG("Slave %i: offline.\n", slave->ring_position);
}
ec_fsm_master_action_next_slave_state(fsm);
return;
}
// slave responded
new_state = EC_READ_U8(datagram->data);
if (!slave->online) { // slave was offline before
slave->online = 1;
slave->error_flag = 0; // clear error flag
slave->current_state = new_state;
if (slave->master->debug_level) {
char cur_state[EC_STATE_STRING_SIZE];
ec_state_string(slave->current_state, cur_state);
EC_DBG("Slave %i: online (%s).\n",
slave->ring_position, cur_state);
}
}
else if (new_state != slave->current_state) {
if (slave->master->debug_level) {
char old_state[EC_STATE_STRING_SIZE],
cur_state[EC_STATE_STRING_SIZE];
ec_state_string(slave->current_state, old_state);
ec_state_string(new_state, cur_state);
EC_DBG("Slave %i: %s -> %s.\n",
slave->ring_position, old_state, cur_state);
}
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);
}
/*****************************************************************************/
/**
Master state: VALIDATE_VENDOR.
Validates the vendor ID of a slave.
*/
void ec_fsm_master_validate_vendor(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
fsm->slave->error_flag = 1;
EC_ERR("Failed to validate vendor ID of slave %i.\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
if (EC_READ_U32(fsm->fsm_sii.value) != slave->sii_vendor_id) {
EC_ERR("Slave %i has an invalid vendor ID!\n", slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
// vendor ID is ok. check product code.
fsm->master_state = ec_fsm_master_validate_product;
ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x000A, EC_FSM_SII_POSITION);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute immediately
}
/*****************************************************************************/
/**
Master action: ADDRESS.
Looks for slave, that have lost their configuration and writes
their station address, so that they can be reconfigured later.
*/
void ec_fsm_master_action_addresses(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
while (fsm->slave->online) {
if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
fsm->master_state = ec_fsm_master_start;
fsm->master_state(fsm); // execute immediately
return;
}
// check next slave
fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
}
if (fsm->master->debug_level)
EC_DBG("Reinitializing slave %i.\n", fsm->slave->ring_position);
// write station address
ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
EC_WRITE_U16(datagram->data, fsm->slave->station_address);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->master_state = ec_fsm_master_rewrite_addresses;
}
/*****************************************************************************/
/**
Master state: VALIDATE_PRODUCT.
Validates the product ID of a slave.
*/
void ec_fsm_master_validate_product(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
fsm->slave->error_flag = 1;
EC_ERR("Failed to validate product code of slave %i.\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
if (EC_READ_U32(fsm->fsm_sii.value) != slave->sii_product_code) {
EC_ERR("Slave %i: invalid product code!\n", slave->ring_position);
EC_ERR("expected 0x%08X, got 0x%08X.\n", slave->sii_product_code,
EC_READ_U32(fsm->fsm_sii.value));
fsm->master_state = ec_fsm_master_error;
return;
}
// have all states been validated?
if (slave->list.next == &fsm->master->slaves) {
fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list);
// start writing addresses to offline slaves
ec_fsm_master_action_addresses(fsm);
return;
}
// validate next slave
fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
fsm->master_state = ec_fsm_master_validate_vendor;
ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x0008, EC_FSM_SII_POSITION);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute immediately
}
/*****************************************************************************/
/**
Master state: REWRITE ADDRESS.
Checks, if the new station address has been written to the slave.
*/
void ec_fsm_master_rewrite_addresses(ec_fsm_t *fsm
/**< finite state machine */
)
{
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = &fsm->datagram;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive address datagram for slave %i.\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
if (datagram->working_counter != 1) {
EC_ERR("Failed to write station address - slave %i did not respond.\n",
slave->ring_position);
fsm->master_state = ec_fsm_master_error;
return;
}
if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
fsm->master_state = ec_fsm_master_start;
fsm->master_state(fsm); // execute immediately
return;
}
// check next slave
fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
// Write new station address to slave
ec_fsm_master_action_addresses(fsm);
}
/*****************************************************************************/
/**
Master state: SCAN SLAVES.
Executes the sub-statemachine for the scanning of a slave.
*/
void ec_fsm_master_scan_slaves(ec_fsm_t *fsm /**< finite state machine */)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave = fsm->slave;
fsm->slave_state(fsm); // execute slave state machine
if (fsm->slave_state != ec_fsm_slave_state_end
&& fsm->slave_state != ec_fsm_slave_state_error) return;
// another slave to fetch?
if (slave->list.next != &master->slaves) {
fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
fsm->slave_state = ec_fsm_slavescan_start;
fsm->slave_state(fsm); // execute immediately
return;
}
EC_INFO("Bus scanning completed.\n");
ec_master_calc_addressing(master);
// set initial states of all slaves to PREOP to make mailbox
// communication possible
list_for_each_entry(slave, &master->slaves, list) {
ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
}
fsm->master_state = ec_fsm_master_end;
}
/*****************************************************************************/
/**
Master state: CONFIGURE SLAVES.
Starts configuring a slave.
*/
void ec_fsm_master_configure_slave(ec_fsm_t *fsm
/**< finite state machine */
)
{
fsm->slave_state(fsm); // execute slave's state machine
if (fsm->slave_state != ec_fsm_slave_state_end
&& fsm->slave_state != ec_fsm_slave_state_error) return;
ec_fsm_master_action_process_states(fsm);
}
/*****************************************************************************/
/**
Master state: WRITE EEPROM.
*/
void ec_fsm_master_write_eeprom(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
fsm->slave->error_flag = 1;
EC_ERR("Failed to write EEPROM contents to slave %i.\n",
slave->ring_position);
kfree(slave->new_eeprom_data);
slave->new_eeprom_data = NULL;
fsm->master_state = ec_fsm_master_error;
return;
}
fsm->sii_offset++;
if (fsm->sii_offset < slave->new_eeprom_size) {
ec_fsm_sii_write(&fsm->fsm_sii, slave, fsm->sii_offset,
slave->new_eeprom_data + fsm->sii_offset,
EC_FSM_SII_NODE);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute immediately
return;
}
// finished writing EEPROM
EC_INFO("Finished writing EEPROM of slave %i.\n", slave->ring_position);
kfree(slave->new_eeprom_data);
slave->new_eeprom_data = NULL;
// TODO: Evaluate new EEPROM contents!
// restart master state machine.
fsm->master_state = ec_fsm_master_start;
fsm->master_state(fsm); // execute immediately
}
/*****************************************************************************/
/**
Master state: SDODICT.
*/
void ec_fsm_master_sdodict(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = fsm->master;
if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
fsm->master_state = ec_fsm_master_error;
return;
}
// SDO dictionary fetching finished
if (master->debug_level) {
unsigned int sdo_count, entry_count;
ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count);
EC_DBG("Fetched %i SDOs and %i entries from slave %i.\n",
sdo_count, entry_count, slave->ring_position);
}
// restart master state machine.
fsm->master_state = ec_fsm_master_start;
fsm->master_state(fsm); // execute immediately
}
/*****************************************************************************/
/**
Master state: SDO REQUEST.
*/
void ec_fsm_master_sdo_request(ec_fsm_t *fsm /**< finite state machine */)
{
ec_master_t *master = fsm->master;
ec_sdo_request_t *request = fsm->sdo_request;
if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
request->return_code = -1;
master->sdo_seq_master++;
fsm->master_state = ec_fsm_master_error;
return;
}
// SDO dictionary fetching finished
request->return_code = 1;
master->sdo_seq_master++;
// restart master state machine.
fsm->master_state = ec_fsm_master_start;
fsm->master_state(fsm); // execute immediately
}
/*****************************************************************************/
/**
State: ERROR.
*/
void ec_fsm_master_error(ec_fsm_t *fsm /**< finite state machine */)
{
fsm->master_state = ec_fsm_master_start;
}
/*****************************************************************************/
/**
State: END.
*/
void ec_fsm_master_end(ec_fsm_t *fsm /**< finite state machine */)
{
fsm->master_state = ec_fsm_master_start;
}
/******************************************************************************
* slave scan state machine
*****************************************************************************/
/**
Slave scan state: START.
First state of the slave state machine. Writes the station address to the
slave, according to its ring position.
*/
void ec_fsm_slavescan_start(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
// write station address
ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
EC_WRITE_U16(datagram->data, fsm->slave->station_address);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_address;
}
/*****************************************************************************/
/**
Slave scan state: ADDRESS.
*/
void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, &fsm->datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive station address datagram for slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to write station address - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
// Read AL state
ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_state;
}
/*****************************************************************************/
/**
Slave scan state: STATE.
*/
void ec_fsm_slavescan_state(ec_fsm_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--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive AL state datagram from slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read AL state - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
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);
EC_WARN("Slave %i has state error bit set (%s)!\n",
slave->ring_position, state_str);
}
// read base data
ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6);
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_base;
}
/*****************************************************************************/
/**
Slave scan state: BASE.
*/
void ec_fsm_slavescan_base(ec_fsm_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--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive base data datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read base data - slave %i did not respond.\n",
slave->ring_position);
return;
}
slave->base_type = EC_READ_U8 (datagram->data);
slave->base_revision = EC_READ_U8 (datagram->data + 1);
slave->base_build = EC_READ_U16(datagram->data + 2);
slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4);
slave->base_sync_count = EC_READ_U8 (datagram->data + 5);
if (slave->base_fmmu_count > EC_MAX_FMMUS)
slave->base_fmmu_count = EC_MAX_FMMUS;
// read data link status
ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2);
ec_master_queue_datagram(slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slavescan_datalink;
}
/*****************************************************************************/
/**
Slave scan state: DATALINK.
*/
void ec_fsm_slavescan_datalink(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
uint16_t dl_status;
unsigned int i;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive DL status datagram from slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read DL status - slave %i did not respond.\n",
slave->ring_position);
return;
}
dl_status = EC_READ_U16(datagram->data);
for (i = 0; i < 4; i++) {
slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;
slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;
slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;
}
// Start fetching EEPROM size
fsm->sii_offset = 0x0040; // first category header
ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);
fsm->slave_state = ec_fsm_slavescan_eeprom_size;
fsm->slave_state(fsm); // execute state immediately
}
/*****************************************************************************/
/**
Slave scan state: EEPROM SIZE.
*/
void ec_fsm_slavescan_eeprom_size(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
uint16_t cat_type, cat_size;
if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to read EEPROM size of slave %i.\n",
slave->ring_position);
return;
}
cat_type = EC_READ_U16(fsm->fsm_sii.value);
cat_size = EC_READ_U16(fsm->fsm_sii.value + 2);
if (cat_type != 0xFFFF) { // not the last category
fsm->sii_offset += cat_size + 2;
ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
EC_FSM_SII_NODE);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
return;
}
slave->eeprom_size = (fsm->sii_offset + 1) * 2;
if (slave->eeprom_data) {
EC_INFO("Freeing old EEPROM data on slave %i...\n",
slave->ring_position);
kfree(slave->eeprom_data);
}
if (!(slave->eeprom_data =
(uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to allocate EEPROM data on slave %i.\n",
slave->ring_position);
return;
}
// Start fetching EEPROM contents
fsm->slave_state = ec_fsm_slavescan_eeprom_data;
fsm->sii_offset = 0x0000;
ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
}
/*****************************************************************************/
/**
Slave scan state: EEPROM DATA.
*/
void ec_fsm_slavescan_eeprom_data(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
uint16_t *cat_word, cat_type, cat_size;
if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to fetch EEPROM contents of slave %i.\n",
slave->ring_position);
return;
}
// 2 words fetched
if (fsm->sii_offset + 2 <= slave->eeprom_size / 2) { // 2 words fit
memcpy(slave->eeprom_data + fsm->sii_offset * 2,
fsm->fsm_sii.value, 4);
}
else { // copy the last word
memcpy(slave->eeprom_data + fsm->sii_offset * 2,
fsm->fsm_sii.value, 2);
}
if (fsm->sii_offset + 2 < slave->eeprom_size / 2) {
// fetch the next 2 words
fsm->sii_offset += 2;
ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
EC_FSM_SII_NODE);
ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
return;
}
// Evaluate EEPROM contents
slave->sii_alias =
EC_READ_U16(slave->eeprom_data + 2 * 0x0004);
slave->sii_vendor_id =
EC_READ_U32(slave->eeprom_data + 2 * 0x0008);
slave->sii_product_code =
EC_READ_U32(slave->eeprom_data + 2 * 0x000A);
slave->sii_revision_number =
EC_READ_U32(slave->eeprom_data + 2 * 0x000C);
slave->sii_serial_number =
EC_READ_U32(slave->eeprom_data + 2 * 0x000E);
slave->sii_rx_mailbox_offset =
EC_READ_U16(slave->eeprom_data + 2 * 0x0018);
slave->sii_rx_mailbox_size =
EC_READ_U16(slave->eeprom_data + 2 * 0x0019);
slave->sii_tx_mailbox_offset =
EC_READ_U16(slave->eeprom_data + 2 * 0x001A);
slave->sii_tx_mailbox_size =
EC_READ_U16(slave->eeprom_data + 2 * 0x001B);
slave->sii_mailbox_protocols =
EC_READ_U16(slave->eeprom_data + 2 * 0x001C);
// evaluate category data
cat_word = (uint16_t *) slave->eeprom_data + 0x0040;
while (EC_READ_U16(cat_word) != 0xFFFF) {
cat_type = EC_READ_U16(cat_word) & 0x7FFF;
cat_size = EC_READ_U16(cat_word + 1);
switch (cat_type) {
case 0x000A:
if (ec_slave_fetch_strings(slave, (uint8_t *) (cat_word + 2)))
goto end;
break;
case 0x001E:
ec_slave_fetch_general(slave, (uint8_t *) (cat_word + 2));
break;
case 0x0028:
break;
case 0x0029:
if (ec_slave_fetch_sync(slave, (uint8_t *) (cat_word + 2),
cat_size))
goto end;
break;
case 0x0032:
if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2),
cat_size, EC_TX_PDO))
goto end;
break;
case 0x0033:
if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2),
cat_size, EC_RX_PDO))
goto end;
break;
default:
if (fsm->master->debug_level)
EC_WARN("Unknown category type 0x%04X in slave %i.\n",
cat_type, slave->ring_position);
}
cat_word += cat_size + 2;
}
fsm->slave_state = ec_fsm_slave_state_end;
return;
end:
EC_ERR("Failed to analyze category data.\n");
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
}
/******************************************************************************
* slave configuration state machine
*****************************************************************************/
/**
Slave configuration state: START.
*/
void ec_fsm_slaveconf_state_start(ec_fsm_t *fsm /**< finite state machine */)
{
if (fsm->master->debug_level) {
EC_DBG("Configuring slave %i...\n", fsm->slave->ring_position);
}
ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);
ec_fsm_change_exec(&fsm->fsm_change);
fsm->slave_state = ec_fsm_slaveconf_state_init;
}
/*****************************************************************************/
/**
Slave configuration state: INIT.
*/
void ec_fsm_slaveconf_state_init(ec_fsm_t *fsm /**< finite state machine */)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = &fsm->datagram;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
return;
}
slave->configured = 1;
if (master->debug_level) {
EC_DBG("Slave %i is now in INIT.\n", slave->ring_position);
}
// check and reset CRC fault counters
//ec_slave_check_crc(slave);
// TODO: Implement state machine for CRC checking.
if (!slave->base_fmmu_count) { // skip FMMU configuration
ec_fsm_slaveconf_enter_sync(fsm);
return;
}
if (master->debug_level)
EC_DBG("Clearing FMMU configurations of slave %i...\n",
slave->ring_position);
// clear FMMU configurations
ec_datagram_npwr(datagram, slave->station_address,
0x0600, EC_FMMU_SIZE * slave->base_fmmu_count);
memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
ec_master_queue_datagram(master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_clear_fmmus;
}
/*****************************************************************************/
/**
Slave configuration state: CLEAR FMMU.
*/
void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *fsm
/**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed receive FMMU clearing datagram for slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to clear FMMUs - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
ec_fsm_slaveconf_enter_sync(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_slaveconf_enter_sync(ec_fsm_t *fsm /**< finite state machine */)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = &fsm->datagram;
const ec_sii_sync_t *sync;
ec_sii_sync_t mbox_sync;
// slave is now in INIT
if (slave->current_state == slave->requested_state) {
fsm->slave_state = ec_fsm_slave_state_end; // successful
if (master->debug_level) {
EC_DBG("Finished configuration of slave %i.\n",
slave->ring_position);
}
return;
}
if (!slave->base_sync_count) { // no sync managers
ec_fsm_slaveconf_enter_preop(fsm);
return;
}
if (master->debug_level) {
EC_DBG("Configuring sync managers of slave %i.\n",
slave->ring_position);
}
// configure sync managers
ec_datagram_npwr(datagram, slave->station_address, 0x0800,
EC_SYNC_SIZE * slave->base_sync_count);
memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
if (list_empty(&slave->sii_syncs)) {
if (slave->sii_rx_mailbox_offset && slave->sii_tx_mailbox_offset) {
if (slave->master->debug_level)
EC_DBG("Guessing sync manager settings for slave %i.\n",
slave->ring_position);
mbox_sync.index = 0;
mbox_sync.physical_start_address = slave->sii_tx_mailbox_offset;
mbox_sync.length = slave->sii_tx_mailbox_size;
mbox_sync.control_register = 0x26;
mbox_sync.enable = 0x01;
mbox_sync.est_length = 0;
ec_sync_config(&mbox_sync, slave,
datagram->data + EC_SYNC_SIZE * mbox_sync.index);
mbox_sync.index = 1;
mbox_sync.physical_start_address = slave->sii_rx_mailbox_offset;
mbox_sync.length = slave->sii_rx_mailbox_size;
mbox_sync.control_register = 0x22;
mbox_sync.enable = 0x01;
mbox_sync.est_length = 0;
ec_sync_config(&mbox_sync, slave,
datagram->data + EC_SYNC_SIZE * mbox_sync.index);
}
}
else if (slave->sii_mailbox_protocols) { // mailboxes present
list_for_each_entry(sync, &slave->sii_syncs, list) {
// only configure mailbox sync-managers
if (sync->index != 0 && sync->index != 1) continue;
ec_sync_config(sync, slave,
datagram->data + EC_SYNC_SIZE * sync->index);
}
}
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_sync;
}
/*****************************************************************************/
/**
Slave configuration state: SYNC.
*/
void ec_fsm_slaveconf_state_sync(ec_fsm_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--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive sync manager configuration datagram for"
" slave %i.\n", slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to set sync managers - slave %i did not respond.\n",
slave->ring_position);
return;
}
ec_fsm_slaveconf_enter_preop(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_slaveconf_enter_preop(ec_fsm_t *fsm /**< finite state machine */)
{
fsm->slave_state = ec_fsm_slaveconf_state_preop;
ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP);
ec_fsm_change_exec(&fsm->fsm_change); // execute immediately
}
/*****************************************************************************/
/**
Slave configuration state: PREOP.
*/
void ec_fsm_slaveconf_state_preop(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = fsm->master;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
return;
}
// slave is now in PREOP
slave->jiffies_preop = fsm->datagram.jiffies_received;
if (master->debug_level) {
EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position);
}
if (slave->current_state == slave->requested_state) {
fsm->slave_state = ec_fsm_slave_state_end; // successful
if (master->debug_level) {
EC_DBG("Finished configuration of slave %i.\n",
slave->ring_position);
}
return;
}
ec_fsm_slaveconf_enter_sync2(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = &fsm->datagram;
ec_sii_sync_t *sync;
if (list_empty(&slave->sii_syncs)) {
ec_fsm_slaveconf_enter_fmmu(fsm);
return;
}
// configure sync managers for process data
ec_datagram_npwr(datagram, slave->station_address, 0x0800,
EC_SYNC_SIZE * slave->base_sync_count);
memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
list_for_each_entry(sync, &slave->sii_syncs, list) {
ec_sync_config(sync, slave,
datagram->data + EC_SYNC_SIZE * sync->index);
}
ec_master_queue_datagram(fsm->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_sync2;
}
/*****************************************************************************/
/**
Slave configuration state: SYNC2.
*/
void ec_fsm_slaveconf_state_sync2(ec_fsm_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--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive process data sync manager configuration"
" datagram for slave %i.\n",
slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to set process data sync managers - slave %i did not"
" respond.\n", slave->ring_position);
return;
}
ec_fsm_slaveconf_enter_fmmu(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_datagram_t *datagram = &fsm->datagram;
unsigned int j;
if (!slave->base_fmmu_count) { // skip FMMU configuration
ec_fsm_slaveconf_enter_sdoconf(fsm);
return;
}
// configure FMMUs
ec_datagram_npwr(datagram, slave->station_address,
0x0600, EC_FMMU_SIZE * slave->base_fmmu_count);
memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
for (j = 0; j < slave->fmmu_count; j++) {
ec_fmmu_config(&slave->fmmus[j], slave,
datagram->data + EC_FMMU_SIZE * j);
}
ec_master_queue_datagram(master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->slave_state = ec_fsm_slaveconf_state_fmmu;
}
/*****************************************************************************/
/**
Slave configuration state: FMMU.
*/
void ec_fsm_slaveconf_state_fmmu(ec_fsm_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--) {
ec_master_queue_datagram(fsm->master, datagram);
return;
}
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to receive FMMUs datagram for slave %i.\n",
fsm->slave->ring_position);
return;
}
if (datagram->working_counter != 1) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
EC_ERR("Failed to set FMMUs - slave %i did not respond.\n",
fsm->slave->ring_position);
return;
}
// No CoE configuration to be applied? Jump to SAVEOP state.
if (list_empty(&slave->sdo_confs)) { // skip SDO configuration
ec_fsm_slaveconf_enter_saveop(fsm);
return;
}
ec_fsm_slaveconf_enter_sdoconf(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *fsm /**< finite state machine */)
{
ec_slave_t *slave = fsm->slave;
if (list_empty(&slave->sdo_confs)) { // skip SDO configuration
ec_fsm_slaveconf_enter_saveop(fsm);
return;
}
// start SDO configuration
fsm->slave_state = ec_fsm_slaveconf_state_sdoconf;
fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list);
ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/**
Slave configuration state: SDOCONF.
*/
void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *fsm /**< finite state machine */)
{
if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
return;
}
// Another SDO to configure?
if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) {
fsm->sdodata = list_entry(fsm->sdodata->list.next,
ec_sdo_data_t, list);
ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
return;
}
// All SDOs are now configured.
// set state to SAVEOP
ec_fsm_slaveconf_enter_saveop(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *fsm /**< finite state machine */)
{
fsm->slave_state = ec_fsm_slaveconf_state_saveop;
ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAVEOP);
ec_fsm_change_exec(&fsm->fsm_change); // execute immediately
}
/*****************************************************************************/
/**
Slave configuration state: SAVEOP.
*/
void ec_fsm_slaveconf_state_saveop(ec_fsm_t *fsm /**< finite state machine */)
{
ec_master_t *master = fsm->master;
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;
fsm->slave_state = ec_fsm_slave_state_error;
return;
}
// slave is now in SAVEOP
if (master->debug_level) {
EC_DBG("Slave %i is now in SAVEOP.\n", slave->ring_position);
}
if (fsm->slave->current_state == fsm->slave->requested_state) {
fsm->slave_state = ec_fsm_slave_state_end; // successful
if (master->debug_level) {
EC_DBG("Finished configuration of slave %i.\n",
slave->ring_position);
}
return;
}
// set state to OP
fsm->slave_state = ec_fsm_slaveconf_state_op;
ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP);
ec_fsm_change_exec(&fsm->fsm_change); // execute immediately
}
/*****************************************************************************/
/**
Slave configuration state: OP
*/
void ec_fsm_slaveconf_state_op(ec_fsm_t *fsm /**< finite state machine */)
{
ec_master_t *master = fsm->master;
ec_slave_t *slave = fsm->slave;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
slave->error_flag = 1;
fsm->slave_state = ec_fsm_slave_state_error;
return;
}
// slave is now in OP
if (master->debug_level) {
EC_DBG("Slave %i is now in OP.\n", slave->ring_position);
EC_DBG("Finished configuration of slave %i.\n", slave->ring_position);
}
fsm->slave_state = ec_fsm_slave_state_end; // successful
}
/******************************************************************************
* Common state functions
*****************************************************************************/
/**
State: ERROR.
*/
void ec_fsm_slave_state_error(ec_fsm_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/
/**
State: END.
*/
void ec_fsm_slave_state_end(ec_fsm_t *fsm /**< finite state machine */)
{
}
/*****************************************************************************/