# HG changeset patch # User Florian Pose # Date 1168446726 0 # Node ID f789bdd78b5453004b98bd97eea2be136a1768f6 # Parent 3eea086385225a16852b258f3623801d011aa7d5 Layed out slave state machines in own files fsm_slave.[ch]; renamed fsm.[ch] to fsm_master.[ch] diff -r 3eea08638522 -r f789bdd78b54 master/Kbuild --- a/master/Kbuild Wed Jan 10 10:58:49 2007 +0000 +++ b/master/Kbuild Wed Jan 10 16:32:06 2007 +0000 @@ -36,8 +36,9 @@ obj-m := ec_master.o ec_master-objs := module.o master.o device.o slave.o datagram.o \ - domain.o mailbox.o canopen.o ethernet.o \ - fsm_sii.o fsm_change.o fsm_coe.o fsm.o xmldev.o + domain.o mailbox.o canopen.o ethernet.o \ + fsm_sii.o fsm_change.o fsm_coe.o fsm_slave.o fsm_master.o \ + xmldev.o ifeq ($(EC_DBG_IF),1) ec_master-objs += debug.o diff -r 3eea08638522 -r f789bdd78b54 master/Makefile.am --- a/master/Makefile.am Wed Jan 10 10:58:49 2007 +0000 +++ b/master/Makefile.am Wed Jan 10 16:32:06 2007 +0000 @@ -43,7 +43,8 @@ fsm_sii.c fsm_sii.h \ fsm_change.c fsm_change.h \ fsm_coe.c fsm_coe.h \ - fsm.c fsm.h \ + fsm_slave.c fsm_slave.h \ + fsm_master.c fsm_master.h \ globals.h \ mailbox.c mailbox.h \ master.c master.h \ diff -r 3eea08638522 -r f789bdd78b54 master/fsm.c --- a/master/fsm.c Wed Jan 10 10:58:49 2007 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1831 +0,0 @@ -/****************************************************************************** - * - * $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->self_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->self_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->self_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 */) -{ -} - -/*****************************************************************************/ diff -r 3eea08638522 -r f789bdd78b54 master/fsm.h --- a/master/fsm.h Wed Jan 10 10:58:49 2007 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,103 +0,0 @@ -/****************************************************************************** - * - * $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. -*/ - -/*****************************************************************************/ - -#ifndef __EC_STATES__ -#define __EC_STATES__ - -#include "globals.h" -#include "../include/ecrt.h" -#include "datagram.h" -#include "slave.h" -#include "canopen.h" - -#include "fsm_sii.h" -#include "fsm_change.h" -#include "fsm_coe.h" - -/*****************************************************************************/ - -typedef struct ec_fsm ec_fsm_t; /**< \see ec_fsm */ - -/** - Finite state machine of an EtherCAT master. -*/ - -struct ec_fsm -{ - ec_master_t *master; /**< master the FSM runs on */ - ec_slave_t *slave; /**< slave the FSM runs on */ - ec_datagram_t datagram; /**< datagram used in the state machine */ - unsigned int retries; /**< retries on datagram timeout. */ - - void (*master_state)(ec_fsm_t *); /**< master state function */ - unsigned int master_slaves_responding; /**< number of responding slaves */ - ec_slave_state_t master_slave_states; /**< states of responding slaves */ - unsigned int master_validation; /**< non-zero, if validation to do */ - uint16_t sii_offset; /**< current offset for SII access */ - ec_sdo_request_t *sdo_request; - - void (*slave_state)(ec_fsm_t *); /**< slave state function */ - ec_sdo_data_t *sdodata; /**< SDO configuration data */ - - ec_fsm_sii_t fsm_sii; /**< SII state machine */ - ec_fsm_change_t fsm_change; /**< State change state machine */ - ec_fsm_coe_t fsm_coe; /**< CoE state machine */ -}; - -/*****************************************************************************/ - -int ec_fsm_init(ec_fsm_t *, ec_master_t *); -void ec_fsm_clear(ec_fsm_t *); - -int ec_fsm_exec(ec_fsm_t *); -int ec_fsm_running(ec_fsm_t *); -int ec_fsm_success(ec_fsm_t *); - -// TODO: layout slave state machines - -/** \cond */ -void ec_fsm_slaveconf_state_start(ec_fsm_t *); -void ec_fsm_slave_state_end(ec_fsm_t *); -void ec_fsm_slave_state_error(ec_fsm_t *); -/** \endcond */ - -/*****************************************************************************/ - -#endif diff -r 3eea08638522 -r f789bdd78b54 master/fsm_master.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_master.c Wed Jan 10 16:32:06 2007 +0000 @@ -0,0 +1,873 @@ +/****************************************************************************** + * + * $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 "master.h" +#include "mailbox.h" +#include "fsm_master.h" + +/*****************************************************************************/ + +void ec_fsm_master_state_start(ec_fsm_master_t *); +void ec_fsm_master_state_broadcast(ec_fsm_master_t *); +void ec_fsm_master_state_read_states(ec_fsm_master_t *); +void ec_fsm_master_state_acknowledge(ec_fsm_master_t *); +void ec_fsm_master_state_validate_vendor(ec_fsm_master_t *); +void ec_fsm_master_state_validate_product(ec_fsm_master_t *); +void ec_fsm_master_state_rewrite_addresses(ec_fsm_master_t *); +void ec_fsm_master_state_configure_slave(ec_fsm_master_t *); +void ec_fsm_master_state_scan_slaves(ec_fsm_master_t *); +void ec_fsm_master_state_write_eeprom(ec_fsm_master_t *); +void ec_fsm_master_state_sdodict(ec_fsm_master_t *); +void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); +void ec_fsm_master_state_end(ec_fsm_master_t *); +void ec_fsm_master_state_error(ec_fsm_master_t *); + +/*****************************************************************************/ + +/** + Constructor. +*/ + +void ec_fsm_master_init(ec_fsm_master_t *fsm, /**< master state machine */ + ec_master_t *master, /**< EtherCAT master */ + ec_datagram_t *datagram /**< datagram object to use */ + ) +{ + fsm->master = master; + fsm->datagram = datagram; + fsm->state = ec_fsm_master_state_start; + fsm->slaves_responding = 0; + fsm->slave_states = EC_SLAVE_STATE_UNKNOWN; + fsm->validate = 0; + + // init sub-state-machines + ec_fsm_slave_init(&fsm->fsm_slave, fsm->datagram); + 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); +} + +/*****************************************************************************/ + +/** + Destructor. +*/ + +void ec_fsm_master_clear(ec_fsm_master_t *fsm /**< master state machine */) +{ + // clear sub-state machines + ec_fsm_slave_clear(&fsm->fsm_slave); + ec_fsm_sii_clear(&fsm->fsm_sii); + ec_fsm_change_clear(&fsm->fsm_change); + ec_fsm_coe_clear(&fsm->fsm_coe); +} + +/*****************************************************************************/ + +/** + 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_master_exec(ec_fsm_master_t *fsm /**< master 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_master_running(fsm); + } + + fsm->state(fsm); + return ec_fsm_master_running(fsm); +} + +/*****************************************************************************/ + +/** + \return false, if state machine has terminated +*/ + +int ec_fsm_master_running(ec_fsm_master_t *fsm /**< master state machine */) +{ + return fsm->state != ec_fsm_master_state_end + && fsm->state != ec_fsm_master_state_error; +} + +/*****************************************************************************/ + +/** + \return true, if the master state machine terminated gracefully +*/ + +int ec_fsm_master_success(ec_fsm_master_t *fsm /**< master state machine */) +{ + return fsm->state == ec_fsm_master_state_end; +} + +/****************************************************************************** + * operation/idle state machine + *****************************************************************************/ + +/** + Master state: START. + Starts with getting slave count and slave states. +*/ + +void ec_fsm_master_state_start(ec_fsm_master_t *fsm) +{ + ec_datagram_brd(fsm->datagram, 0x0130, 2); + ec_master_queue_datagram(fsm->master, fsm->datagram); + fsm->state = ec_fsm_master_state_broadcast; +} + +/*****************************************************************************/ + +/** + Master state: BROADCAST. + Processes the broadcast read slave count and slaves states. +*/ + +void ec_fsm_master_state_broadcast(ec_fsm_master_t *fsm /**< master 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->slaves_responding = 0; + list_for_each_entry(slave, &master->slaves, list) { + slave->online = 0; + } + fsm->state = ec_fsm_master_state_error; + return; + } + + topology_change = (datagram->working_counter != + fsm->slaves_responding); + states_change = (EC_READ_U8(datagram->data) != fsm->slave_states); + + fsm->slave_states = EC_READ_U8(datagram->data); + fsm->slaves_responding = datagram->working_counter; + + if (topology_change) { + EC_INFO("%i slave%s responding.\n", + fsm->slaves_responding, + fsm->slaves_responding == 1 ? "" : "s"); + + if (master->mode == EC_MASTER_MODE_OPERATION) { + if (fsm->slaves_responding == master->slave_count) { + fsm->validate = 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->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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_error; + return; + } + + if (ec_slave_init(slave, master, i, i + 1)) { + // freeing of "slave" already done + ec_master_destroy_slaves(master); + fsm->state = ec_fsm_master_state_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); + ec_fsm_slave_start_scan(&fsm->fsm_slave, slave); + ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately + fsm->state = ec_fsm_master_state_scan_slaves; + 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->state = ec_fsm_master_state_read_states; +} + +/*****************************************************************************/ + +/** + Master action: PROC_STATES. + Processes the slave states. +*/ + +void ec_fsm_master_action_process_states(ec_fsm_master_t *fsm + /**< master 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->self_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->self_configured) { + EC_DBG("Reconfiguring slave %i (%s).\n", + slave->ring_position, old_state); + } + } + + fsm->slave = slave; + ec_fsm_slave_start_conf(&fsm->fsm_slave, slave); + ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately + fsm->state = ec_fsm_master_state_configure_slave; + 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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_write_eeprom; + fsm->state(fsm); // execute immediately + return; + } + } + + fsm->state = ec_fsm_master_state_end; +} + +/*****************************************************************************/ + +/** + Master action: Get state of next slave. +*/ + +void ec_fsm_master_action_next_slave_state(ec_fsm_master_t *fsm + /**< master 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->state = ec_fsm_master_state_read_states; + return; + } + + // all slave states read + + // check, if a bus validation has to be done + if (fsm->validate) { + fsm->validate = 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->state = ec_fsm_master_state_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_state_read_states(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_acknowledge; + return; + } + + ec_fsm_master_action_next_slave_state(fsm); +} + +/*****************************************************************************/ + +/** + Master state: ACKNOWLEDGE +*/ + +void ec_fsm_master_state_acknowledge(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_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_state_validate_vendor(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_error; + return; + } + + // vendor ID is ok. check product code. + fsm->state = ec_fsm_master_state_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_master_t *fsm /**< master state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + + while (fsm->slave->online) { + if (fsm->slave->list.next == &fsm->master->slaves) { // last slave? + fsm->state = ec_fsm_master_state_start; + fsm->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->state = ec_fsm_master_state_rewrite_addresses; +} + +/*****************************************************************************/ + +/** + Master state: VALIDATE_PRODUCT. + Validates the product ID of a slave. +*/ + +void ec_fsm_master_state_validate_product(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_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_state_rewrite_addresses(ec_fsm_master_t *fsm + /**< master 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->state = ec_fsm_master_state_error; + return; + } + + if (datagram->working_counter != 1) { + EC_ERR("Failed to write station address - slave %i did not respond.\n", + slave->ring_position); + fsm->state = ec_fsm_master_state_error; + return; + } + + if (fsm->slave->list.next == &fsm->master->slaves) { // last slave? + fsm->state = ec_fsm_master_state_start; + fsm->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_state_scan_slaves(ec_fsm_master_t *fsm /**< master state machine */) +{ + ec_master_t *master = fsm->master; + ec_slave_t *slave = fsm->slave; + + if (ec_fsm_slave_exec(&fsm->fsm_slave)) // execute slave state machine + return; + + // another slave to fetch? + if (slave->list.next != &master->slaves) { + fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); + ec_fsm_slave_start_scan(&fsm->fsm_slave, slave); + ec_fsm_slave_exec(&fsm->fsm_slave); // 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->state = ec_fsm_master_state_end; +} + +/*****************************************************************************/ + +/** + Master state: CONFIGURE SLAVES. + Starts configuring a slave. +*/ + +void ec_fsm_master_state_configure_slave(ec_fsm_master_t *fsm + /**< master state machine */ + ) +{ + if (ec_fsm_slave_exec(&fsm->fsm_slave)) // execute slave's state machine + return; + + ec_fsm_master_action_process_states(fsm); +} + +/*****************************************************************************/ + +/** + Master state: WRITE EEPROM. +*/ + +void ec_fsm_master_state_write_eeprom(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_start; + fsm->state(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + Master state: SDODICT. +*/ + +void ec_fsm_master_state_sdodict(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_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->state = ec_fsm_master_state_start; + fsm->state(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + Master state: SDO REQUEST. +*/ + +void ec_fsm_master_state_sdo_request(ec_fsm_master_t *fsm /**< master 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->state = ec_fsm_master_state_error; + return; + } + + // SDO dictionary fetching finished + + request->return_code = 1; + master->sdo_seq_master++; + + // restart master state machine. + fsm->state = ec_fsm_master_state_start; + fsm->state(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + State: ERROR. +*/ + +void ec_fsm_master_state_error( + ec_fsm_master_t *fsm /**< master state machine */ + ) +{ + fsm->state = ec_fsm_master_state_start; +} + +/*****************************************************************************/ + +/** + State: END. +*/ + +void ec_fsm_master_state_end(ec_fsm_master_t *fsm /**< master state machine */) +{ + fsm->state = ec_fsm_master_state_start; +} + +/*****************************************************************************/ + diff -r 3eea08638522 -r f789bdd78b54 master/fsm_master.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_master.h Wed Jan 10 16:32:06 2007 +0000 @@ -0,0 +1,91 @@ +/****************************************************************************** + * + * $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. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_MASTER__ +#define __EC_FSM_MASTER__ + +#include "globals.h" +#include "../include/ecrt.h" +#include "datagram.h" +#include "slave.h" +#include "canopen.h" + +#include "fsm_slave.h" + +/*****************************************************************************/ + +typedef struct ec_fsm_master ec_fsm_master_t; /**< \see ec_fsm_master */ + +/** + Finite state machine of an EtherCAT master. +*/ + +struct ec_fsm_master +{ + ec_master_t *master; /**< master the FSM runs on */ + ec_datagram_t *datagram; /**< datagram used in the state machine */ + unsigned int retries; /**< retries on datagram timeout. */ + + void (*state)(ec_fsm_master_t *); /**< master state function */ + unsigned int slaves_responding; /**< number of responding slaves */ + ec_slave_state_t slave_states; /**< states of responding slaves */ + unsigned int validate; /**< non-zero, if validation to do */ + ec_slave_t *slave; /**< current slave */ + ec_sdo_request_t *sdo_request; /**< SDO request to process */ + uint16_t sii_offset; + + ec_fsm_slave_t fsm_slave; /**< slave state machine */ + ec_fsm_sii_t fsm_sii; /**< SII state machine */ + ec_fsm_change_t fsm_change; /**< State change state machine */ + ec_fsm_coe_t fsm_coe; /**< CoE state machine */ +}; + +/*****************************************************************************/ + +void ec_fsm_master_init(ec_fsm_master_t *, ec_master_t *, ec_datagram_t *); +void ec_fsm_master_clear(ec_fsm_master_t *); + +int ec_fsm_master_exec(ec_fsm_master_t *); +int ec_fsm_master_running(ec_fsm_master_t *); +int ec_fsm_master_success(ec_fsm_master_t *); + +/*****************************************************************************/ + +#endif diff -r 3eea08638522 -r f789bdd78b54 master/fsm_slave.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_slave.c Wed Jan 10 16:32:06 2007 +0000 @@ -0,0 +1,1101 @@ +/****************************************************************************** + * + * $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 slave state machines. +*/ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "mailbox.h" +#include "fsm_slave.h" + +/*****************************************************************************/ + +void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *); +void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *); +void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *); +void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *); +void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *); +void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *); +void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *); + +void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *); + +void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *); + +void ec_fsm_slave_state_end(ec_fsm_slave_t *); +void ec_fsm_slave_state_error(ec_fsm_slave_t *); + +/*****************************************************************************/ + +/** + Constructor. +*/ + +void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */ + ec_datagram_t *datagram /**< datagram structure to use */ + ) +{ + fsm->datagram = datagram; + + // 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); +} + +/*****************************************************************************/ + +/** + Destructor. +*/ + +void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave 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); +} + +/*****************************************************************************/ + +/** + * Start slave scan state machine. + */ + +void ec_fsm_slave_start_scan(ec_fsm_slave_t *fsm, /**< slave state machine */ + ec_slave_t *slave /**< slave to configure */ + ) +{ + fsm->slave = slave; + fsm->state = ec_fsm_slave_scan_state_start; +} + +/*****************************************************************************/ + +/** + * Start slave configuration state machine. + */ + +void ec_fsm_slave_start_conf(ec_fsm_slave_t *fsm, /**< slave state machine */ + ec_slave_t *slave /**< slave to configure */ + ) +{ + fsm->slave = slave; + fsm->state = ec_fsm_slave_conf_state_start; +} + +/*****************************************************************************/ + +/** + \return false, if state machine has terminated +*/ + +int ec_fsm_slave_running(const ec_fsm_slave_t *fsm /**< slave state machine */) +{ + return fsm->state != ec_fsm_slave_state_end + && fsm->state != ec_fsm_slave_state_error; +} + +/*****************************************************************************/ + +/** + 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_slave_exec(ec_fsm_slave_t *fsm /**< slave 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_slave_running(fsm); + } + + fsm->state(fsm); + return ec_fsm_slave_running(fsm); +} + +/*****************************************************************************/ + +/** + \return true, if the state machine terminated gracefully +*/ + +int ec_fsm_slave_success(const ec_fsm_slave_t *fsm /**< slave state machine */) +{ + return fsm->state == ec_fsm_slave_state_end; +} + +/****************************************************************************** + * 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_slave_scan_state_start(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + // write station address + ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2); + EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address); + ec_master_queue_datagram(fsm->slave->master, fsm->datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_scan_state_address; +} + +/*****************************************************************************/ + +/** + Slave scan state: ADDRESS. +*/ + +void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { + ec_master_queue_datagram(fsm->slave->master, fsm->datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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->slave->master, datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_scan_state_state; +} + +/*****************************************************************************/ + +/** + Slave scan state: STATE. +*/ + +void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *fsm /**< slave 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->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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->slave->master, datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_scan_state_base; +} + +/*****************************************************************************/ + +/** + Slave scan state: BASE. +*/ + +void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *fsm /**< slave 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->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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->state = ec_fsm_slave_scan_state_datalink; +} + +/*****************************************************************************/ + +/** + Slave scan state: DATALINK. +*/ + +void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *fsm /**< slave 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->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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->state = ec_fsm_slave_scan_state_eeprom_size; + fsm->state(fsm); // execute state immediately +} + +/*****************************************************************************/ + +/** + Slave scan state: EEPROM SIZE. +*/ + +void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *fsm /**< slave 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->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->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->state = ec_fsm_slave_scan_state_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_slave_scan_state_eeprom_data(ec_fsm_slave_t *fsm /**< slave 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->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->slave->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->state = ec_fsm_slave_state_end; + return; + +end: + EC_ERR("Failed to analyze category data.\n"); + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; +} + +/****************************************************************************** + * slave configuration state machine + *****************************************************************************/ + +/** + Slave configuration state: START. +*/ + +void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + if (fsm->slave->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->state = ec_fsm_slave_conf_state_init; +} + +/*****************************************************************************/ + +/** + Slave configuration state: INIT. +*/ + +void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_master_t *master = fsm->slave->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->state = ec_fsm_slave_state_error; + return; + } + + slave->self_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_slave_conf_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->state = ec_fsm_slave_conf_state_clear_fmmus; +} + +/*****************************************************************************/ + +/** + Slave configuration state: CLEAR FMMU. +*/ + +void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *fsm + /**< slave state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { + ec_master_queue_datagram(fsm->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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_slave_conf_enter_sync(fsm); +} + +/*****************************************************************************/ + +/** +*/ + +void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_master_t *master = fsm->slave->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->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_slave_conf_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->slave->master, datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_conf_state_sync; +} + +/*****************************************************************************/ + +/** + Slave configuration state: SYNC. +*/ + +void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *fsm /**< slave 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->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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_slave_conf_enter_preop(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + fsm->state = ec_fsm_slave_conf_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_slave_conf_state_preop(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = fsm->slave->master; + + if (ec_fsm_change_exec(&fsm->fsm_change)) return; + + if (!ec_fsm_change_success(&fsm->fsm_change)) { + slave->error_flag = 1; + fsm->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->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_slave_conf_enter_sync2(fsm); +} + +/*****************************************************************************/ + +/** +*/ + +void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *fsm /**< slave 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_slave_conf_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->slave->master, datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_conf_state_sync2; +} + +/*****************************************************************************/ + +/** + Slave configuration state: SYNC2. +*/ + +void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *fsm /**< slave 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->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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_slave_conf_enter_fmmu(fsm); +} + +/*****************************************************************************/ + +/** +*/ + +void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *fsm /**< slave 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_slave_conf_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->state = ec_fsm_slave_conf_state_fmmu; +} + +/*****************************************************************************/ + +/** + Slave configuration state: FMMU. +*/ + +void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *fsm /**< slave 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->slave->master, datagram); + return; + } + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->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->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_slave_conf_enter_saveop(fsm); + return; + } + + ec_fsm_slave_conf_enter_sdoconf(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + + if (list_empty(&slave->sdo_confs)) { // skip SDO configuration + ec_fsm_slave_conf_enter_saveop(fsm); + return; + } + + // start SDO configuration + fsm->state = ec_fsm_slave_conf_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_slave_conf_state_sdoconf(ec_fsm_slave_t *fsm /**< slave 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->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_slave_conf_enter_saveop(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + fsm->state = ec_fsm_slave_conf_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_slave_conf_state_saveop(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_master_t *master = fsm->slave->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->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->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->state = ec_fsm_slave_conf_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_slave_conf_state_op(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_master_t *master = fsm->slave->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->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->state = ec_fsm_slave_state_end; // successful +} + +/****************************************************************************** + * Common state functions + *****************************************************************************/ + +/** + State: ERROR. +*/ + +void ec_fsm_slave_state_error(ec_fsm_slave_t *fsm /**< slave state machine */) +{ +} + +/*****************************************************************************/ + +/** + State: END. +*/ + +void ec_fsm_slave_state_end(ec_fsm_slave_t *fsm /**< slave state machine */) +{ +} + +/*****************************************************************************/ diff -r 3eea08638522 -r f789bdd78b54 master/fsm_slave.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_slave.h Wed Jan 10 16:32:06 2007 +0000 @@ -0,0 +1,88 @@ +/****************************************************************************** + * + * $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. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_SLAVE__ +#define __EC_FSM_SLAVE__ + +#include "globals.h" +#include "../include/ecrt.h" +#include "datagram.h" +#include "slave.h" +#include "fsm_sii.h" +#include "fsm_change.h" +#include "fsm_coe.h" + +/*****************************************************************************/ + +typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */ + +/** + Finite state machine of an EtherCAT slave. +*/ + +struct ec_fsm_slave +{ + ec_slave_t *slave; /**< slave the FSM runs on */ + ec_datagram_t *datagram; /**< datagram used in the state machine */ + unsigned int retries; /**< retries on datagram timeout. */ + + void (*state)(ec_fsm_slave_t *); /**< state function */ + ec_sdo_data_t *sdodata; /**< SDO configuration data */ + uint16_t sii_offset; + + ec_fsm_sii_t fsm_sii; /**< SII state machine */ + ec_fsm_change_t fsm_change; /**< State change state machine */ + ec_fsm_coe_t fsm_coe; /**< CoE state machine */ +}; + +/*****************************************************************************/ + +void ec_fsm_slave_init(ec_fsm_slave_t *, ec_datagram_t *); +void ec_fsm_slave_clear(ec_fsm_slave_t *); + +void ec_fsm_slave_start_scan(ec_fsm_slave_t *, ec_slave_t *); +void ec_fsm_slave_start_conf(ec_fsm_slave_t *, ec_slave_t *); + +int ec_fsm_slave_exec(ec_fsm_slave_t *); +int ec_fsm_slave_success(const ec_fsm_slave_t *); + +/*****************************************************************************/ + +#endif diff -r 3eea08638522 -r f789bdd78b54 master/master.c --- a/master/master.c Wed Jan 10 10:58:49 2007 +0000 +++ b/master/master.c Wed Jan 10 16:32:06 2007 +0000 @@ -183,8 +183,15 @@ list_add_tail(&eoe->list, &master->eoe_handlers); } + // init state machine datagram + ec_datagram_init(&master->fsm_datagram); + if (ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE)) { + EC_ERR("Failed to allocate FSM datagram.\n"); + goto out_clear_eoe; + } + // create state machine object - if (ec_fsm_init(&master->fsm, master)) goto out_clear_eoe; + ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); // init kobject and add it to the hierarchy memset(&master->kobj, 0x00, sizeof(struct kobject)); @@ -203,14 +210,14 @@ return 0; - out_clear_eoe: +out_clear_eoe: list_for_each_entry_safe(eoe, next_eoe, &master->eoe_handlers, list) { list_del(&eoe->list); ec_eoe_clear(eoe); kfree(eoe); } ec_xmldev_clear(&master->xmldev); - out_return: +out_return: return -1; } @@ -252,7 +259,8 @@ list_del_init(&datagram->queue); } - ec_fsm_clear(&master->fsm); + ec_fsm_master_clear(&master->fsm); + ec_datagram_clear(&master->fsm_datagram); ec_xmldev_clear(&master->xmldev); // clear EoE objects @@ -416,7 +424,6 @@ int ec_master_enter_operation_mode(ec_master_t *master /**< EtherCAT master */) { ec_slave_t *slave; - ec_datagram_t *datagram = &master->fsm.datagram; ec_master_eoe_stop(master); // stop EoE timer master->eoe_checked = 1; // prevent from starting again by FSM @@ -425,15 +432,15 @@ master->mode = EC_MASTER_MODE_OPERATION; // wait for FSM datagram - if (datagram->state == EC_DATAGRAM_SENT) { - while (get_cycles() - datagram->cycles_sent + if (master->fsm_datagram.state == EC_DATAGRAM_SENT) { + while (get_cycles() - master->fsm_datagram.cycles_sent < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)) {} ecrt_master_receive(master); } // finish running master FSM - if (ec_fsm_running(&master->fsm)) { - while (ec_fsm_exec(&master->fsm)) { + if (ec_fsm_master_running(&master->fsm)) { + while (ec_fsm_master_exec(&master->fsm)) { ec_master_sync_io(master); } } @@ -477,42 +484,41 @@ /**< EtherCAT master */) { ec_slave_t *slave; - ec_fsm_t *fsm = &master->fsm; - ec_datagram_t *datagram = &master->fsm.datagram; + ec_fsm_master_t *fsm = &master->fsm; + ec_fsm_slave_t fsm_slave; ec_master_eoe_stop(master); // stop EoE timer master->eoe_checked = 1; // prevent from starting again by FSM // wait for FSM datagram - if (datagram->state == EC_DATAGRAM_SENT) { + if (master->fsm_datagram.state == EC_DATAGRAM_SENT) { // active waiting - while (get_cycles() - datagram->cycles_sent + while (get_cycles() - master->fsm_datagram.cycles_sent < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)); ecrt_master_receive(master); } // finish running master FSM - if (ec_fsm_running(fsm)) { - while (ec_fsm_exec(fsm)) { + if (ec_fsm_master_running(fsm)) { + while (ec_fsm_master_exec(fsm)) { ec_master_sync_io(master); } } + ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram); + // set states for all slaves list_for_each_entry(slave, &master->slaves, list) { ec_slave_reset(slave); ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); - fsm->slave = slave; - fsm->slave_state = ec_fsm_slaveconf_state_start; - - do { - fsm->slave_state(fsm); + ec_fsm_slave_start_conf(&fsm_slave, slave); + while (ec_fsm_slave_exec(&fsm_slave)) { ec_master_sync_io(master); } - while (fsm->slave_state != ec_fsm_slave_state_end - && fsm->slave_state != ec_fsm_slave_state_error); - } + } + + ec_fsm_slave_clear(&fsm_slave); ec_master_destroy_domains(master); @@ -816,7 +822,7 @@ spin_unlock_bh(&master->internal_lock); // execute master state machine - ec_fsm_exec(&master->fsm); + ec_fsm_master_exec(&master->fsm); // send spin_lock_bh(&master->internal_lock); @@ -1402,7 +1408,7 @@ { uint32_t domain_offset; ec_domain_t *domain; - ec_fsm_t *fsm = &master->fsm; + ec_fsm_slave_t fsm_slave; ec_slave_t *slave; // allocate all domains @@ -1415,24 +1421,23 @@ domain_offset += domain->data_size; } + ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram); + // configure all slaves list_for_each_entry(slave, &master->slaves, list) { - fsm->slave = slave; - fsm->slave_state = ec_fsm_slaveconf_state_start; - - do { - fsm->slave_state(fsm); + ec_fsm_slave_start_conf(&fsm_slave, slave); + while (ec_fsm_slave_exec(&fsm_slave)) { ec_master_sync_io(master); } - while (fsm->slave_state != ec_fsm_slave_state_end - && fsm->slave_state != ec_fsm_slave_state_error); - - if (fsm->slave_state == ec_fsm_slave_state_error) { + + if (!ec_fsm_slave_success(&fsm_slave)) { + ec_fsm_slave_clear(&fsm_slave); EC_ERR("Failed to configure slave %i!\n", slave->ring_position); return -1; } } + ec_fsm_slave_clear(&fsm_slave); ec_master_prepare(master); // prepare asynchronous IO return 0; @@ -1537,7 +1542,7 @@ ec_master_output_stats(master); // execute master state machine in a loop - ec_fsm_exec(&master->fsm); + ec_fsm_master_exec(&master->fsm); } /*****************************************************************************/ diff -r 3eea08638522 -r f789bdd78b54 master/master.h --- a/master/master.h Wed Jan 10 10:58:49 2007 +0000 +++ b/master/master.h Wed Jan 10 16:32:06 2007 +0000 @@ -50,7 +50,7 @@ #include "device.h" #include "domain.h" #include "xmldev.h" -#include "fsm.h" +#include "fsm_master.h" /*****************************************************************************/ @@ -104,7 +104,8 @@ ec_xmldev_t xmldev; /**< XML character device */ - ec_fsm_t fsm; /**< master state machine */ + ec_fsm_master_t fsm; /**< master state machine */ + ec_datagram_t fsm_datagram; /**< datagram used for state machines */ ec_master_mode_t mode; /**< master mode */ struct list_head slaves; /**< list of slaves on the bus */