fp@238: /****************************************************************************** fp@238: * fp@238: * $Id$ fp@238: * fp@238: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@238: * fp@238: * This file is part of the IgH EtherCAT Master. fp@238: * fp@238: * The IgH EtherCAT Master is free software; you can redistribute it fp@238: * and/or modify it under the terms of the GNU General Public License fp@246: * as published by the Free Software Foundation; either version 2 of the fp@246: * License, or (at your option) any later version. fp@238: * fp@238: * The IgH EtherCAT Master is distributed in the hope that it will be fp@238: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@238: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@238: * GNU General Public License for more details. fp@238: * fp@238: * You should have received a copy of the GNU General Public License fp@238: * along with the IgH EtherCAT Master; if not, write to the Free Software fp@238: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@238: * fp@246: * The right to use EtherCAT Technology is granted and comes free of fp@246: * charge under condition of compatibility of product made by fp@246: * Licensee. People intending to distribute/sell products based on the fp@246: * code, have to sign an agreement to guarantee that products using fp@246: * software based on IgH EtherCAT master stay compatible with the actual fp@246: * EtherCAT specification (which are released themselves as an open fp@246: * standard) as the (only) precondition to have the right to use EtherCAT fp@246: * Technology, IP and trade marks. fp@246: * fp@238: *****************************************************************************/ fp@238: fp@238: /** fp@238: \file fp@238: EtherCAT finite state machines. fp@238: */ fp@238: fp@238: /*****************************************************************************/ fp@238: fp@238: #include "globals.h" fp@238: #include "fsm.h" fp@238: #include "master.h" fp@329: #include "mailbox.h" fp@238: fp@238: /*****************************************************************************/ fp@238: fp@238: void ec_fsm_master_start(ec_fsm_t *); fp@260: void ec_fsm_master_broadcast(ec_fsm_t *); fp@304: void ec_fsm_master_read_states(ec_fsm_t *); fp@260: void ec_fsm_master_validate_vendor(ec_fsm_t *); fp@260: void ec_fsm_master_validate_product(ec_fsm_t *); fp@304: void ec_fsm_master_rewrite_addresses(ec_fsm_t *); fp@304: void ec_fsm_master_configure_slave(ec_fsm_t *); fp@304: void ec_fsm_master_scan_slaves(ec_fsm_t *); fp@304: void ec_fsm_master_write_eeprom(ec_fsm_t *); fp@251: fp@325: void ec_fsm_startup_start(ec_fsm_t *); fp@325: void ec_fsm_startup_broadcast(ec_fsm_t *); fp@325: void ec_fsm_startup_scan(ec_fsm_t *); fp@325: fp@325: void ec_fsm_configuration_start(ec_fsm_t *); fp@325: void ec_fsm_configuration_conf(ec_fsm_t *); fp@325: fp@308: void ec_fsm_slavescan_start(ec_fsm_t *); fp@310: void ec_fsm_slavescan_address(ec_fsm_t *); fp@308: void ec_fsm_slavescan_state(ec_fsm_t *); fp@308: void ec_fsm_slavescan_base(ec_fsm_t *); fp@308: void ec_fsm_slavescan_datalink(ec_fsm_t *); fp@308: void ec_fsm_slavescan_eeprom_size(ec_fsm_t *); fp@308: void ec_fsm_slavescan_eeprom_data(ec_fsm_t *); fp@308: fp@309: void ec_fsm_slaveconf_init(ec_fsm_t *); fp@308: void ec_fsm_slaveconf_sync(ec_fsm_t *); fp@308: void ec_fsm_slaveconf_preop(ec_fsm_t *); fp@308: void ec_fsm_slaveconf_fmmu(ec_fsm_t *); fp@329: void ec_fsm_slaveconf_sdoconf(ec_fsm_t *); fp@308: void ec_fsm_slaveconf_saveop(ec_fsm_t *); fp@308: void ec_fsm_slaveconf_op(ec_fsm_t *); fp@238: fp@238: void ec_fsm_sii_start_reading(ec_fsm_t *); fp@269: void ec_fsm_sii_read_check(ec_fsm_t *); fp@269: void ec_fsm_sii_read_fetch(ec_fsm_t *); fp@269: void ec_fsm_sii_start_writing(ec_fsm_t *); fp@269: void ec_fsm_sii_write_check(ec_fsm_t *); fp@269: void ec_fsm_sii_write_check2(ec_fsm_t *); fp@238: fp@251: void ec_fsm_change_start(ec_fsm_t *); fp@251: void ec_fsm_change_check(ec_fsm_t *); fp@251: void ec_fsm_change_status(ec_fsm_t *); fp@251: void ec_fsm_change_code(ec_fsm_t *); fp@251: void ec_fsm_change_ack(ec_fsm_t *); fp@311: void ec_fsm_change_check_ack(ec_fsm_t *); fp@325: fp@329: void ec_fsm_coe_down_start(ec_fsm_t *); fp@329: void ec_fsm_coe_down_request(ec_fsm_t *); fp@329: void ec_fsm_coe_down_check(ec_fsm_t *); fp@339: void ec_fsm_coe_down_response(ec_fsm_t *); fp@329: fp@325: void ec_fsm_end(ec_fsm_t *); fp@325: void ec_fsm_error(ec_fsm_t *); fp@251: fp@329: void ec_canopen_abort_msg(uint32_t); fp@329: fp@238: /*****************************************************************************/ fp@238: fp@260: /** fp@260: Constructor. fp@260: */ fp@260: fp@260: int ec_fsm_init(ec_fsm_t *fsm, /**< finite state machine */ fp@260: ec_master_t *master /**< EtherCAT master */ fp@325: ) fp@238: { fp@238: fsm->master = master; fp@238: fsm->master_state = ec_fsm_master_start; fp@238: fsm->master_slaves_responding = 0; fp@238: fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN; fp@260: fsm->master_validation = 0; fp@238: fp@293: ec_datagram_init(&fsm->datagram); fp@293: if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) { fp@293: EC_ERR("Failed to allocate FSM datagram.\n"); fp@238: return -1; fp@238: } fp@238: fp@238: return 0; fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@260: /** fp@260: Destructor. fp@260: */ fp@260: fp@260: void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_clear(&fsm->datagram); fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@260: /** fp@260: Resets the state machine. fp@260: */ fp@260: fp@260: void ec_fsm_reset(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@238: fsm->master_state = ec_fsm_master_start; fp@238: fsm->master_slaves_responding = 0; fp@238: fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN; fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@260: /** fp@260: Executes the current state of the state machine. fp@260: */ fp@260: fp@260: void ec_fsm_execute(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@238: fsm->master_state(fsm); fp@238: } fp@238: fp@325: /*****************************************************************************/ fp@325: fp@343: /** fp@343: Initializes the master startup state machine. fp@343: */ fp@343: fp@325: void ec_fsm_startup(ec_fsm_t *fsm) fp@325: { fp@325: fsm->master_state = ec_fsm_startup_start; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@343: /** fp@343: Returns the running state of the master startup state machine. fp@343: \return non-zero if not terminated yet. fp@343: */ fp@343: fp@343: int ec_fsm_startup_running(ec_fsm_t *fsm /**< Finite state machine */) fp@325: { fp@325: return fsm->master_state != ec_fsm_end && fp@325: fsm->master_state != ec_fsm_error; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@343: /** fp@343: Returns, if the master startup state machine terminated with success. fp@343: \return non-zero if successful. fp@343: */ fp@343: fp@343: int ec_fsm_startup_success(ec_fsm_t *fsm /**< Finite state machine */) fp@325: { fp@325: return fsm->master_state == ec_fsm_end; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@343: /** fp@343: Initializes the master configuration state machine. fp@343: */ fp@343: fp@325: void ec_fsm_configuration(ec_fsm_t *fsm) fp@325: { fp@325: fsm->master_state = ec_fsm_configuration_start; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@343: /** fp@343: Returns the running state of the master configuration state machine. fp@343: \return non-zero if not terminated yet. fp@343: */ fp@343: fp@343: int ec_fsm_configuration_running(ec_fsm_t *fsm /**< Finite state machine */) fp@325: { fp@325: return fsm->master_state != ec_fsm_end && fp@325: fsm->master_state != ec_fsm_error; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@343: /** fp@343: Returns, if the master confuguration state machine terminated with success. fp@343: \return non-zero if successful. fp@343: */ fp@343: fp@343: int ec_fsm_configuration_success(ec_fsm_t *fsm /**< Finite state machine */) fp@325: { fp@325: return fsm->master_state == ec_fsm_end; fp@325: } fp@325: fp@238: /****************************************************************************** fp@325: * master startup state machine fp@325: *****************************************************************************/ fp@325: fp@325: /** fp@325: Master state: START. fp@325: Starts with getting slave count and slave states. fp@325: */ fp@325: fp@325: void ec_fsm_startup_start(ec_fsm_t *fsm) fp@325: { fp@325: ec_datagram_brd(&fsm->datagram, 0x0130, 2); fp@325: ec_master_queue_datagram(fsm->master, &fsm->datagram); fp@325: fsm->master_state = ec_fsm_startup_broadcast; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@325: /** fp@325: Master state: BROADCAST. fp@325: Processes the broadcast read slave count and slaves states. fp@325: */ fp@325: fp@325: void ec_fsm_startup_broadcast(ec_fsm_t *fsm /**< finite state machine */) fp@325: { fp@325: ec_datagram_t *datagram = &fsm->datagram; fp@325: unsigned int i; fp@325: ec_slave_t *slave; fp@325: ec_master_t *master = fsm->master; fp@325: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@397: EC_ERR("Failed to receive broadcast datagram.\n"); fp@325: fsm->master_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: EC_INFO("Scanning bus.\n"); fp@325: fp@325: ec_master_clear_slaves(master); fp@325: fp@325: master->slave_count = datagram->working_counter; fp@325: fp@325: if (!master->slave_count) { fp@325: // no slaves present -> finish state machine. fp@325: fsm->master_state = ec_fsm_end; fp@325: return; fp@325: } fp@325: fp@325: // init slaves fp@325: for (i = 0; i < master->slave_count; i++) { fp@325: if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t), fp@325: GFP_KERNEL))) { fp@325: EC_ERR("Failed to allocate slave %i!\n", i); fp@325: fsm->master_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (ec_slave_init(slave, master, i, i + 1)) { fp@325: fsm->master_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (kobject_add(&slave->kobj)) { fp@325: EC_ERR("Failed to add kobject.\n"); fp@325: kobject_put(&slave->kobj); // free fp@325: fsm->master_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: list_add_tail(&slave->list, &master->slaves); fp@325: } fp@325: fp@325: // begin scanning of slaves fp@325: fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); fp@325: fsm->slave_state = ec_fsm_slavescan_start; fp@325: fsm->master_state = ec_fsm_startup_scan; fp@325: fsm->master_state(fsm); // execute immediately fp@325: return; fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@325: /** fp@325: Master state: SCAN. fp@325: Executes the sub-statemachine for the scanning of a slave. fp@325: */ fp@325: fp@325: void ec_fsm_startup_scan(ec_fsm_t *fsm /**< finite state machine */) fp@325: { fp@325: ec_master_t *master = fsm->master; fp@325: ec_slave_t *slave = fsm->slave; fp@325: fp@325: fsm->slave_state(fsm); // execute slave state machine fp@325: fp@325: if (fsm->slave_state == ec_fsm_error) { fp@325: EC_ERR("Slave scanning failed.\n"); fp@325: fsm->master_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (fsm->slave_state != ec_fsm_end) return; fp@325: fp@325: // another slave to scan? fp@325: if (slave->list.next != &master->slaves) { fp@325: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@325: fsm->slave_state = ec_fsm_slavescan_start; fp@325: fsm->slave_state(fsm); // execute immediately fp@325: return; fp@325: } fp@325: fp@325: EC_INFO("Bus scanning completed.\n"); fp@325: fp@325: ec_master_calc_addressing(master); fp@325: fp@325: fsm->master_state = ec_fsm_end; fp@325: } fp@325: fp@325: /****************************************************************************** fp@325: * master configuration state machine fp@325: *****************************************************************************/ fp@325: fp@343: /** fp@343: Master configuration state machine: START. fp@343: */ fp@343: fp@325: void ec_fsm_configuration_start(ec_fsm_t *fsm /**< finite state machine */) fp@325: { fp@325: ec_master_t *master = fsm->master; fp@325: fp@325: if (list_empty(&master->slaves)) { fp@325: fsm->master_state = ec_fsm_end; fp@325: return; fp@325: } fp@325: fp@325: // begin configuring slaves fp@325: fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); fp@325: fsm->slave_state = ec_fsm_slaveconf_init; fp@325: fsm->change_new = EC_SLAVE_STATE_INIT; fp@325: fsm->change_state = ec_fsm_change_start; fp@325: fsm->master_state = ec_fsm_configuration_conf; fp@325: fsm->master_state(fsm); // execute immediately fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@325: /** fp@325: Master state: CONF. fp@325: */ fp@325: fp@325: void ec_fsm_configuration_conf(ec_fsm_t *fsm /**< finite state machine */) fp@325: { fp@325: ec_master_t *master = fsm->master; fp@325: ec_slave_t *slave = fsm->slave; fp@325: fp@325: fsm->slave_state(fsm); // execute slave's state machine fp@325: fp@325: if (fsm->slave_state == ec_fsm_error) { fp@325: fsm->master_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (fsm->slave_state != ec_fsm_end) return; fp@325: fp@325: // another slave to configure? fp@325: if (slave->list.next != &master->slaves) { fp@325: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@325: fsm->slave_state = ec_fsm_slaveconf_init; fp@325: fsm->change_new = EC_SLAVE_STATE_INIT; fp@325: fsm->change_state = ec_fsm_change_start; fp@325: fsm->master_state(fsm); // execute immediately fp@325: return; fp@325: } fp@325: fp@325: fsm->master_state = ec_fsm_end; fp@325: } fp@325: fp@325: /****************************************************************************** fp@325: * operation / idle state machine fp@238: *****************************************************************************/ fp@238: fp@238: /** fp@260: Master state: START. fp@238: Starts with getting slave count and slave states. fp@238: */ fp@238: fp@238: void ec_fsm_master_start(ec_fsm_t *fsm) fp@238: { fp@293: ec_datagram_brd(&fsm->datagram, 0x0130, 2); fp@293: ec_master_queue_datagram(fsm->master, &fsm->datagram); fp@260: fsm->master_state = ec_fsm_master_broadcast; fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@260: Master state: BROADCAST. fp@260: Processes the broadcast read slave count and slaves states. fp@260: */ fp@260: fp@260: void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */) fp@260: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@260: unsigned int topology_change, states_change, i; fp@238: ec_slave_t *slave; fp@260: ec_master_t *master = fsm->master; fp@238: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@260: if (!master->device->link_state) { fp@254: fsm->master_slaves_responding = 0; fp@260: list_for_each_entry(slave, &master->slaves, list) { fp@260: slave->online = 0; fp@260: } fp@260: } fp@238: fsm->master_state = ec_fsm_master_start; fp@238: fsm->master_state(fsm); // execute immediately fp@238: return; fp@238: } fp@238: fp@293: topology_change = (datagram->working_counter != fp@260: fsm->master_slaves_responding); fp@293: states_change = (EC_READ_U8(datagram->data) != fsm->master_slave_states); fp@293: fp@293: fsm->master_slave_states = EC_READ_U8(datagram->data); fp@293: fsm->master_slaves_responding = datagram->working_counter; fp@260: fp@260: if (topology_change) { fp@262: EC_INFO("%i slave%s responding.\n", fp@262: fsm->master_slaves_responding, fp@262: fsm->master_slaves_responding == 1 ? "" : "s"); fp@262: fp@325: if (master->mode == EC_MASTER_MODE_OPERATION) { fp@262: if (fsm->master_slaves_responding == master->slave_count) { fp@262: fsm->master_validation = 1; // start validation later fp@262: } fp@262: else { fp@262: EC_WARN("Invalid slave count. Bus in tainted state.\n"); fp@262: } fp@260: } fp@260: } fp@260: fp@260: if (states_change) { fp@403: char states[EC_STATE_STRING_SIZE]; fp@325: ec_state_string(fsm->master_slave_states, states); fp@325: EC_INFO("Slave states: %s.\n", states); fp@260: } fp@260: fp@306: // topology change in idle mode: clear all slaves and scan the bus fp@306: if (topology_change && master->mode == EC_MASTER_MODE_IDLE) { fp@260: EC_INFO("Scanning bus.\n"); fp@260: fp@260: ec_master_eoe_stop(master); fp@260: ec_master_clear_slaves(master); fp@260: fp@325: master->slave_count = datagram->working_counter; fp@325: fp@325: if (!master->slave_count) { fp@260: // no slaves present -> finish state machine. fp@260: fsm->master_state = ec_fsm_master_start; fp@251: fsm->master_state(fsm); // execute immediately fp@251: return; fp@251: } fp@251: fp@260: // init slaves fp@325: for (i = 0; i < master->slave_count; i++) { fp@260: if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t), fp@260: GFP_ATOMIC))) { fp@260: EC_ERR("Failed to allocate slave %i!\n", i); fp@325: ec_master_clear_slaves(master); fp@260: fsm->master_state = ec_fsm_master_start; fp@260: fsm->master_state(fsm); // execute immediately fp@260: return; fp@260: } fp@260: fp@260: if (ec_slave_init(slave, master, i, i + 1)) { fp@325: // freeing of "slave" already done fp@325: ec_master_clear_slaves(master); fp@260: fsm->master_state = ec_fsm_master_start; fp@260: fsm->master_state(fsm); // execute immediately fp@260: return; fp@260: } fp@260: fp@260: if (kobject_add(&slave->kobj)) { fp@260: EC_ERR("Failed to add kobject.\n"); fp@260: kobject_put(&slave->kobj); // free fp@325: ec_master_clear_slaves(master); fp@260: fsm->master_state = ec_fsm_master_start; fp@260: fsm->master_state(fsm); // execute immediately fp@260: return; fp@260: } fp@260: fp@260: list_add_tail(&slave->list, &master->slaves); fp@260: } fp@260: fp@260: // begin scanning of slaves fp@260: fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); fp@308: fsm->slave_state = ec_fsm_slavescan_start; fp@304: fsm->master_state = ec_fsm_master_scan_slaves; fp@260: fsm->master_state(fsm); // execute immediately fp@260: return; fp@260: } fp@260: fp@260: // fetch state from each slave fp@260: fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); fp@293: ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2); fp@293: ec_master_queue_datagram(master, &fsm->datagram); fp@304: fsm->master_state = ec_fsm_master_read_states; fp@304: } fp@304: fp@304: /*****************************************************************************/ fp@304: fp@304: /** fp@304: Master action: PROC_STATES. fp@304: Processes the slave states. fp@304: */ fp@304: fp@304: void ec_fsm_master_action_process_states(ec_fsm_t *fsm fp@304: /**< finite state machine */ fp@304: ) fp@304: { fp@304: ec_master_t *master = fsm->master; fp@304: ec_slave_t *slave; fp@403: char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE]; fp@304: fp@304: // check if any slaves are not in the state, they're supposed to be fp@304: list_for_each_entry(slave, &master->slaves, list) { fp@304: if (slave->error_flag || fp@304: !slave->online || fp@304: slave->requested_state == EC_SLAVE_STATE_UNKNOWN || fp@304: slave->current_state == slave->requested_state) continue; fp@304: fp@325: ec_state_string(slave->current_state, old_state); fp@325: ec_state_string(slave->requested_state, new_state); fp@325: EC_INFO("Changing state of slave %i from %s to %s.\n", fp@325: slave->ring_position, old_state, new_state); fp@304: fp@304: fsm->slave = slave; fp@309: fsm->slave_state = ec_fsm_slaveconf_init; fp@304: fsm->change_new = EC_SLAVE_STATE_INIT; fp@304: fsm->change_state = ec_fsm_change_start; fp@304: fsm->master_state = ec_fsm_master_configure_slave; fp@304: fsm->master_state(fsm); // execute immediately fp@304: return; fp@304: } fp@304: fp@325: // Check, if EoE processing has to be started fp@325: ec_master_eoe_start(master); fp@325: fp@306: if (master->mode == EC_MASTER_MODE_IDLE) { fp@304: // nothing to configure. check for pending EEPROM write operations. fp@304: list_for_each_entry(slave, &master->slaves, list) { fp@304: if (!slave->new_eeprom_data) continue; fp@304: fp@304: if (!slave->online || slave->error_flag) { fp@304: kfree(slave->new_eeprom_data); fp@304: slave->new_eeprom_data = NULL; fp@304: EC_ERR("Discarding EEPROM data, slave %i not ready.\n", fp@304: slave->ring_position); fp@304: continue; fp@304: } fp@304: fp@304: // found pending EEPROM write operation. execute it! fp@304: EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position); fp@304: fsm->sii_offset = 0x0000; fp@304: memcpy(fsm->sii_value, slave->new_eeprom_data, 2); fp@304: fsm->sii_mode = 1; fp@304: fsm->sii_state = ec_fsm_sii_start_writing; fp@304: fsm->slave = slave; fp@304: fsm->master_state = ec_fsm_master_write_eeprom; fp@304: fsm->master_state(fsm); // execute immediately fp@304: return; fp@304: } fp@304: } fp@304: fp@304: // nothing to do. restart master state machine. fp@304: fsm->master_state = ec_fsm_master_start; fp@304: fsm->master_state(fsm); // execute immediately fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@260: Master action: Get state of next slave. fp@260: */ fp@260: fp@260: void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm fp@260: /**< finite state machine */) fp@260: { fp@260: ec_master_t *master = fsm->master; fp@260: ec_slave_t *slave = fsm->slave; fp@260: fp@304: // is there another slave to query? fp@292: if (slave->list.next != &master->slaves) { fp@292: // process next slave fp@292: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@293: ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, fp@293: 0x0130, 2); fp@293: ec_master_queue_datagram(master, &fsm->datagram); fp@304: fsm->master_state = ec_fsm_master_read_states; fp@304: return; fp@304: } fp@304: fp@304: // all slave states read fp@304: fp@304: // check, if a bus validation has to be done fp@292: if (fsm->master_validation) { fp@292: fsm->master_validation = 0; fp@292: list_for_each_entry(slave, &master->slaves, list) { fp@292: if (slave->online) continue; fp@292: fp@292: // At least one slave is offline. validate! fp@292: EC_INFO("Validating bus.\n"); fp@292: fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); fp@292: fsm->master_state = ec_fsm_master_validate_vendor; fp@292: fsm->sii_offset = 0x0008; // vendor ID fp@292: fsm->sii_mode = 0; fp@292: fsm->sii_state = ec_fsm_sii_start_reading; fp@292: fsm->sii_state(fsm); // execute immediately fp@292: return; fp@251: } fp@292: } fp@292: fp@304: ec_fsm_master_action_process_states(fsm); fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@260: Master state: STATES. fp@260: Fetches the AL- and online state of a slave. fp@260: */ fp@260: fp@304: void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */) fp@260: { fp@260: ec_slave_t *slave = fsm->slave; fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@260: uint8_t new_state; fp@260: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@238: fsm->master_state = ec_fsm_master_start; fp@238: fsm->master_state(fsm); // execute immediately fp@238: return; fp@238: } fp@238: fp@260: // did the slave not respond to its station address? fp@293: if (datagram->working_counter != 1) { fp@260: if (slave->online) { fp@260: slave->online = 0; fp@260: EC_INFO("Slave %i: offline.\n", slave->ring_position); fp@260: } fp@260: ec_fsm_master_action_next_slave_state(fsm); fp@260: return; fp@260: } fp@260: fp@260: // slave responded fp@293: new_state = EC_READ_U8(datagram->data); fp@260: if (!slave->online) { // slave was offline before fp@403: char cur_state[EC_STATE_STRING_SIZE]; fp@260: slave->online = 1; fp@291: slave->error_flag = 0; // clear error flag fp@260: slave->current_state = new_state; fp@325: ec_state_string(slave->current_state, cur_state); fp@325: EC_INFO("Slave %i: online (%s).\n", slave->ring_position, cur_state); fp@260: } fp@260: else if (new_state != slave->current_state) { fp@403: char old_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE]; fp@325: ec_state_string(slave->current_state, old_state); fp@325: ec_state_string(new_state, cur_state); fp@325: EC_INFO("Slave %i: %s -> %s.\n", fp@325: slave->ring_position, old_state, cur_state); fp@260: slave->current_state = new_state; fp@260: } fp@260: fp@260: ec_fsm_master_action_next_slave_state(fsm); fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@260: Master state: VALIDATE_VENDOR. fp@260: Validates the vendor ID of a slave. fp@260: */ fp@260: fp@260: void ec_fsm_master_validate_vendor(ec_fsm_t *fsm /**< finite state machine */) fp@260: { fp@260: ec_slave_t *slave = fsm->slave; fp@260: fp@260: fsm->sii_state(fsm); // execute SII state machine fp@260: fp@325: if (fsm->sii_state == ec_fsm_error) { fp@292: fsm->slave->error_flag = 1; fp@260: EC_ERR("Failed to validate vendor ID of slave %i.\n", fp@260: slave->ring_position); fp@238: fsm->master_state = ec_fsm_master_start; fp@238: fsm->master_state(fsm); // execute immediately fp@238: return; fp@238: } fp@238: fp@325: if (fsm->sii_state != ec_fsm_end) return; fp@260: fp@269: if (EC_READ_U32(fsm->sii_value) != slave->sii_vendor_id) { fp@260: EC_ERR("Slave %i: invalid vendor ID!\n", slave->ring_position); fp@238: fsm->master_state = ec_fsm_master_start; fp@238: fsm->master_state(fsm); // execute immediately fp@238: return; fp@238: } fp@238: fp@260: // vendor ID is ok. check product code. fp@260: fsm->master_state = ec_fsm_master_validate_product; fp@260: fsm->sii_offset = 0x000A; // product code fp@260: fsm->sii_mode = 0; fp@260: fsm->sii_state = ec_fsm_sii_start_reading; fp@260: fsm->sii_state(fsm); // execute immediately fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@304: Master action: ADDRESS. fp@260: Looks for slave, that have lost their configuration and writes fp@260: their station address, so that they can be reconfigured later. fp@260: */ fp@260: fp@304: void ec_fsm_master_action_addresses(ec_fsm_t *fsm /**< finite state machine */) fp@260: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@260: fp@260: while (fsm->slave->online) { fp@260: if (fsm->slave->list.next == &fsm->master->slaves) { // last slave? fp@251: fsm->master_state = ec_fsm_master_start; fp@260: fsm->master_state(fsm); // execute immediately fp@238: return; fp@238: } fp@260: // check next slave fp@260: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@260: } fp@260: fp@260: EC_INFO("Reinitializing slave %i.\n", fsm->slave->ring_position); fp@260: fp@260: // write station address fp@293: ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2); fp@293: EC_WRITE_U16(datagram->data, fsm->slave->station_address); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@304: fsm->master_state = ec_fsm_master_rewrite_addresses; fp@304: } fp@304: fp@304: /*****************************************************************************/ fp@304: fp@304: /** fp@304: Master state: VALIDATE_PRODUCT. fp@304: Validates the product ID of a slave. fp@304: */ fp@304: fp@304: void ec_fsm_master_validate_product(ec_fsm_t *fsm /**< finite state machine */) fp@304: { fp@304: ec_slave_t *slave = fsm->slave; fp@304: fp@304: fsm->sii_state(fsm); // execute SII state machine fp@304: fp@325: if (fsm->sii_state == ec_fsm_error) { fp@304: fsm->slave->error_flag = 1; fp@304: EC_ERR("Failed to validate product code of slave %i.\n", fp@304: slave->ring_position); fp@304: fsm->master_state = ec_fsm_master_start; fp@304: fsm->master_state(fsm); // execute immediately fp@304: return; fp@304: } fp@304: fp@325: if (fsm->sii_state != ec_fsm_end) return; fp@304: fp@304: if (EC_READ_U32(fsm->sii_value) != slave->sii_product_code) { fp@304: EC_ERR("Slave %i: invalid product code!\n", slave->ring_position); fp@304: EC_ERR("expected 0x%08X, got 0x%08X.\n", slave->sii_product_code, fp@304: EC_READ_U32(fsm->sii_value)); fp@304: fsm->master_state = ec_fsm_master_start; fp@304: fsm->master_state(fsm); // execute immediately fp@304: return; fp@304: } fp@304: fp@304: // have all states been validated? fp@304: if (slave->list.next == &fsm->master->slaves) { fp@304: fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list); fp@304: // start writing addresses to offline slaves fp@304: ec_fsm_master_action_addresses(fsm); fp@304: return; fp@304: } fp@304: fp@304: // validate next slave fp@304: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@304: fsm->master_state = ec_fsm_master_validate_vendor; fp@304: fsm->sii_offset = 0x0008; // vendor ID fp@304: fsm->sii_mode = 0; fp@304: fsm->sii_state = ec_fsm_sii_start_reading; fp@304: fsm->sii_state(fsm); // execute immediately fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@260: Master state: ADDRESS. fp@260: Checks, if the new station address has been written to the slave. fp@260: */ fp@260: fp@304: void ec_fsm_master_rewrite_addresses(ec_fsm_t *fsm fp@304: /**< finite state machine */ fp@304: ) fp@260: { fp@260: ec_slave_t *slave = fsm->slave; fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@260: EC_ERR("Failed to write station address on slave %i.\n", fp@260: slave->ring_position); fp@260: } fp@260: fp@260: if (fsm->slave->list.next == &fsm->master->slaves) { // last slave? fp@260: fsm->master_state = ec_fsm_master_start; fp@260: fsm->master_state(fsm); // execute immediately fp@260: return; fp@260: } fp@260: fp@260: // check next slave fp@260: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@304: // Write new station address to slave fp@304: ec_fsm_master_action_addresses(fsm); fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@238: /** fp@260: Master state: SCAN. fp@260: Executes the sub-statemachine for the scanning of a slave. fp@260: */ fp@260: fp@304: void ec_fsm_master_scan_slaves(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@238: ec_master_t *master = fsm->master; fp@251: ec_slave_t *slave = fsm->slave; fp@325: fp@238: fp@238: fsm->slave_state(fsm); // execute slave state machine fp@238: fp@325: if (fsm->slave_state != ec_fsm_end fp@325: && fsm->slave_state != ec_fsm_error) return; fp@325: fp@325: // another slave to fetch? fp@325: if (slave->list.next != &master->slaves) { fp@325: fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); fp@325: fsm->slave_state = ec_fsm_slavescan_start; fp@325: fsm->slave_state(fsm); // execute immediately fp@325: return; fp@325: } fp@325: fp@325: EC_INFO("Bus scanning completed.\n"); fp@325: fp@325: ec_master_calc_addressing(master); fp@325: fp@325: // determine initial states. fp@325: list_for_each_entry(slave, &master->slaves, list) { fp@325: if (ec_slave_is_coupler(slave)) { fp@325: slave->requested_state = EC_SLAVE_STATE_OP; fp@325: } fp@325: else { fp@325: if (master->mode == EC_MASTER_MODE_OPERATION) fp@325: slave->requested_state = EC_SLAVE_STATE_PREOP; fp@325: else fp@325: slave->requested_state = EC_SLAVE_STATE_INIT; fp@325: } fp@325: } fp@325: fp@325: fsm->master_state = ec_fsm_master_start; fp@325: fsm->master_state(fsm); // execute immediately fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@238: /** fp@260: Master state: CONF. fp@260: Starts configuring a slave. fp@260: */ fp@260: fp@304: void ec_fsm_master_configure_slave(ec_fsm_t *fsm fp@304: /**< finite state machine */ fp@304: ) fp@260: { fp@251: fsm->slave_state(fsm); // execute slave's state machine fp@325: fp@325: if (fsm->slave_state != ec_fsm_end fp@325: && fsm->slave_state != ec_fsm_error) return; fp@304: fp@304: ec_fsm_master_action_process_states(fsm); fp@238: } fp@238: fp@269: /*****************************************************************************/ fp@269: fp@269: /** fp@269: Master state: EEPROM. fp@269: */ fp@269: fp@304: void ec_fsm_master_write_eeprom(ec_fsm_t *fsm /**< finite state machine */) fp@269: { fp@269: ec_slave_t *slave = fsm->slave; fp@269: fp@269: fsm->sii_state(fsm); // execute SII state machine fp@269: fp@325: if (fsm->sii_state == ec_fsm_error) { fp@292: fsm->slave->error_flag = 1; fp@269: EC_ERR("Failed to write EEPROM contents to slave %i.\n", fp@269: slave->ring_position); fp@269: kfree(slave->new_eeprom_data); fp@269: slave->new_eeprom_data = NULL; fp@269: fsm->master_state = ec_fsm_master_start; fp@269: fsm->master_state(fsm); // execute immediately fp@269: return; fp@269: } fp@269: fp@325: if (fsm->sii_state != ec_fsm_end) return; fp@269: fp@269: fsm->sii_offset++; fp@269: if (fsm->sii_offset < slave->new_eeprom_size) { fp@269: memcpy(fsm->sii_value, slave->new_eeprom_data + fsm->sii_offset, 2); fp@269: fsm->sii_state = ec_fsm_sii_start_writing; fp@269: fsm->sii_state(fsm); // execute immediately fp@269: return; fp@269: } fp@269: fp@269: // finished writing EEPROM fp@269: EC_INFO("Finished writing EEPROM of slave %i.\n", slave->ring_position); fp@269: kfree(slave->new_eeprom_data); fp@269: slave->new_eeprom_data = NULL; fp@269: fp@325: // TODO: Evaluate new EEPROM contents! fp@325: fp@269: // restart master state machine. fp@325: fsm->master_state = ec_fsm_master_start; fp@269: fsm->master_state(fsm); // execute immediately fp@269: return; fp@269: } fp@269: fp@238: /****************************************************************************** fp@325: * slave scan state machine fp@238: *****************************************************************************/ fp@238: fp@238: /** fp@260: Slave state: START_READING. fp@238: First state of the slave state machine. Writes the station address to the fp@238: slave, according to its ring position. fp@238: */ fp@238: fp@308: void ec_fsm_slavescan_start(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@238: fp@238: // write station address fp@293: ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2); fp@293: EC_WRITE_U16(datagram->data, fsm->slave->station_address); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@310: fsm->slave_state = ec_fsm_slavescan_address; fp@310: } fp@310: fp@310: /*****************************************************************************/ fp@310: fp@310: /** fp@310: Slave state: ADDRESS. fp@310: */ fp@310: fp@310: void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@292: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@292: EC_ERR("Failed to write station address of slave %i.\n", fp@238: fsm->slave->ring_position); fp@292: return; fp@292: } fp@292: fp@292: // Read AL state fp@293: ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@310: fsm->slave_state = ec_fsm_slavescan_state; fp@310: } fp@310: fp@310: /*****************************************************************************/ fp@310: fp@310: /** fp@310: Slave state: STATE. fp@310: */ fp@310: fp@310: void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */) fp@255: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@291: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@292: EC_ERR("Failed to read AL state of slave %i.\n", fp@255: fsm->slave->ring_position); fp@255: return; fp@255: } fp@255: fp@293: slave->current_state = EC_READ_U8(datagram->data); fp@402: if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { fp@292: EC_WARN("Slave %i has state error bit set (0x%02X)!\n", fp@255: slave->ring_position, slave->current_state); fp@255: slave->current_state &= 0x0F; fp@255: } fp@255: fp@238: // read base data fp@293: ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@310: fsm->slave_state = ec_fsm_slavescan_base; fp@310: } fp@310: fp@310: /*****************************************************************************/ fp@310: fp@310: /** fp@310: Slave state: BASE. fp@310: */ fp@310: fp@310: void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@291: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@292: EC_ERR("Failed to read base data of slave %i.\n", fp@238: slave->ring_position); fp@238: return; fp@238: } fp@238: fp@293: slave->base_type = EC_READ_U8 (datagram->data); fp@293: slave->base_revision = EC_READ_U8 (datagram->data + 1); fp@293: slave->base_build = EC_READ_U16(datagram->data + 2); fp@293: slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); fp@293: slave->base_sync_count = EC_READ_U8 (datagram->data + 5); fp@238: fp@238: if (slave->base_fmmu_count > EC_MAX_FMMUS) fp@238: slave->base_fmmu_count = EC_MAX_FMMUS; fp@238: fp@238: // read data link status fp@293: ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2); fp@293: ec_master_queue_datagram(slave->master, datagram); fp@310: fsm->slave_state = ec_fsm_slavescan_datalink; fp@310: } fp@310: fp@310: /*****************************************************************************/ fp@310: fp@310: /** fp@310: Slave state: DATALINK. fp@310: */ fp@310: fp@310: void ec_fsm_slavescan_datalink(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@238: ec_slave_t *slave = fsm->slave; fp@238: uint16_t dl_status; fp@238: unsigned int i; fp@238: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@291: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@292: EC_ERR("Failed to read DL status of slave %i.\n", fp@238: slave->ring_position); fp@238: return; fp@238: } fp@238: fp@293: dl_status = EC_READ_U16(datagram->data); fp@238: for (i = 0; i < 4; i++) { fp@238: slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0; fp@238: slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0; fp@238: slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0; fp@238: } fp@238: fp@266: // Start fetching EEPROM size fp@266: fp@266: fsm->sii_offset = 0x0040; // first category header fp@260: fsm->sii_mode = 1; fp@238: fsm->sii_state = ec_fsm_sii_start_reading; fp@310: fsm->slave_state = ec_fsm_slavescan_eeprom_size; fp@238: fsm->slave_state(fsm); // execute state immediately fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@238: /** fp@310: Slave state: EEPROM_SIZE. fp@310: */ fp@310: fp@310: void ec_fsm_slavescan_eeprom_size(ec_fsm_t *fsm /**< finite state machine */) fp@266: { fp@266: ec_slave_t *slave = fsm->slave; fp@266: uint16_t cat_type, cat_size; fp@238: fp@238: // execute SII state machine fp@238: fsm->sii_state(fsm); fp@238: fp@325: if (fsm->sii_state == ec_fsm_error) { fp@292: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@266: EC_ERR("Failed to read EEPROM size of slave %i.\n", fp@266: slave->ring_position); fp@238: return; fp@238: } fp@238: fp@325: if (fsm->sii_state != ec_fsm_end) return; fp@238: fp@269: cat_type = EC_READ_U16(fsm->sii_value); fp@269: cat_size = EC_READ_U16(fsm->sii_value + 2); fp@266: fp@266: if (cat_type != 0xFFFF) { // not the last category fp@266: fsm->sii_offset += cat_size + 2; fp@266: fsm->sii_state = ec_fsm_sii_start_reading; fp@266: fsm->sii_state(fsm); // execute state immediately fp@266: return; fp@266: } fp@266: fp@266: slave->eeprom_size = (fsm->sii_offset + 1) * 2; fp@266: fp@266: if (slave->eeprom_data) { fp@266: EC_INFO("Freeing old EEPROM data on slave %i...\n", fp@266: slave->ring_position); fp@266: kfree(slave->eeprom_data); fp@266: } fp@266: fp@266: if (!(slave->eeprom_data = fp@266: (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) { fp@291: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@266: EC_ERR("Failed to allocate EEPROM data on slave %i.\n", fp@266: slave->ring_position); fp@266: return; fp@266: } fp@266: fp@266: // Start fetching EEPROM contents fp@266: fp@266: fsm->sii_offset = 0x0000; fp@266: fsm->sii_mode = 1; fp@238: fsm->sii_state = ec_fsm_sii_start_reading; fp@310: fsm->slave_state = ec_fsm_slavescan_eeprom_data; fp@238: fsm->slave_state(fsm); // execute state immediately fp@238: } fp@238: fp@238: /*****************************************************************************/ fp@238: fp@238: /** fp@310: Slave state: EEPROM_DATA. fp@310: */ fp@310: fp@310: void ec_fsm_slavescan_eeprom_data(ec_fsm_t *fsm /**< finite state machine */) fp@266: { fp@266: ec_slave_t *slave = fsm->slave; fp@266: uint16_t *cat_word, cat_type, cat_size; fp@266: fp@238: // execute SII state machine fp@238: fsm->sii_state(fsm); fp@238: fp@325: if (fsm->sii_state == ec_fsm_error) { fp@292: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@266: EC_ERR("Failed to fetch EEPROM contents of slave %i.\n", fp@266: slave->ring_position); fp@238: return; fp@238: } fp@238: fp@325: if (fsm->sii_state != ec_fsm_end) return; fp@238: fp@266: // 2 words fetched fp@266: fp@266: if (fsm->sii_offset + 2 <= slave->eeprom_size / 2) { // 2 words fit fp@269: memcpy(slave->eeprom_data + fsm->sii_offset * 2, fsm->sii_value, 4); fp@266: } fp@266: else { // copy the last word fp@269: memcpy(slave->eeprom_data + fsm->sii_offset * 2, fsm->sii_value, 2); fp@266: } fp@266: fp@266: if (fsm->sii_offset + 2 < slave->eeprom_size / 2) { fp@266: // fetch the next 2 words fp@266: fsm->sii_offset += 2; fp@238: fsm->sii_state = ec_fsm_sii_start_reading; fp@266: fsm->sii_state(fsm); // execute state immediately fp@266: return; fp@266: } fp@266: fp@266: // Evaluate EEPROM contents fp@266: fp@266: slave->sii_alias = fp@266: EC_READ_U16(slave->eeprom_data + 2 * 0x0004); fp@266: slave->sii_vendor_id = fp@266: EC_READ_U32(slave->eeprom_data + 2 * 0x0008); fp@266: slave->sii_product_code = fp@266: EC_READ_U32(slave->eeprom_data + 2 * 0x000A); fp@266: slave->sii_revision_number = fp@266: EC_READ_U32(slave->eeprom_data + 2 * 0x000C); fp@266: slave->sii_serial_number = fp@266: EC_READ_U32(slave->eeprom_data + 2 * 0x000E); fp@266: slave->sii_rx_mailbox_offset = fp@266: EC_READ_U16(slave->eeprom_data + 2 * 0x0018); fp@266: slave->sii_rx_mailbox_size = fp@266: EC_READ_U16(slave->eeprom_data + 2 * 0x0019); fp@266: slave->sii_tx_mailbox_offset = fp@266: EC_READ_U16(slave->eeprom_data + 2 * 0x001A); fp@266: slave->sii_tx_mailbox_size = fp@266: EC_READ_U16(slave->eeprom_data + 2 * 0x001B); fp@266: slave->sii_mailbox_protocols = fp@266: EC_READ_U16(slave->eeprom_data + 2 * 0x001C); fp@266: fp@266: // evaluate category data fp@266: cat_word = (uint16_t *) slave->eeprom_data + 0x0040; fp@266: while (EC_READ_U16(cat_word) != 0xFFFF) { fp@266: cat_type = EC_READ_U16(cat_word) & 0x7FFF; fp@266: cat_size = EC_READ_U16(cat_word + 1); fp@266: fp@266: switch (cat_type) { fp@266: case 0x000A: fp@266: if (ec_slave_fetch_strings(slave, (uint8_t *) (cat_word + 2))) fp@266: goto end; fp@266: break; fp@266: case 0x001E: fp@325: ec_slave_fetch_general(slave, (uint8_t *) (cat_word + 2)); fp@266: break; fp@266: case 0x0028: fp@266: break; fp@266: case 0x0029: fp@266: if (ec_slave_fetch_sync(slave, (uint8_t *) (cat_word + 2), fp@266: cat_size)) fp@266: goto end; fp@266: break; fp@266: case 0x0032: fp@266: if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2), fp@266: cat_size, EC_TX_PDO)) fp@266: goto end; fp@266: break; fp@266: case 0x0033: fp@266: if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2), fp@266: cat_size, EC_RX_PDO)) fp@266: goto end; fp@266: break; fp@266: default: fp@266: EC_WARN("Unknown category type 0x%04X in slave %i.\n", fp@266: cat_type, slave->ring_position); fp@266: } fp@266: fp@266: cat_word += cat_size + 2; fp@266: } fp@266: fp@325: fsm->slave_state = ec_fsm_end; fp@325: return; fp@310: fp@292: end: fp@325: EC_ERR("Failed to analyze category data.\n"); fp@291: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@308: } fp@308: fp@308: /****************************************************************************** fp@325: * slave configuration state machine fp@308: *****************************************************************************/ fp@251: fp@251: /** fp@309: Slave state: INIT. fp@309: */ fp@309: fp@309: void ec_fsm_slaveconf_init(ec_fsm_t *fsm /**< finite state machine */) fp@309: { fp@309: ec_slave_t *slave = fsm->slave; fp@309: ec_datagram_t *datagram = &fsm->datagram; fp@325: const ec_sii_sync_t *sync; fp@251: fp@251: fsm->change_state(fsm); // execute state change state machine fp@251: fp@325: if (fsm->change_state == ec_fsm_error) { fp@292: slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (fsm->change_state != ec_fsm_end) return; fp@251: fp@251: // slave is now in INIT fp@251: if (slave->current_state == slave->requested_state) { fp@325: fsm->slave_state = ec_fsm_end; // successful fp@325: return; fp@251: } fp@251: fp@251: // check and reset CRC fault counters fp@251: //ec_slave_check_crc(slave); fp@325: // TODO: Implement state machine for CRC checking. fp@251: fp@251: if (!slave->base_sync_count) { // no sync managers fp@308: fsm->slave_state = ec_fsm_slaveconf_preop; fp@309: fsm->change_new = EC_SLAVE_STATE_PREOP; fp@309: fsm->change_state = ec_fsm_change_start; fp@309: fsm->change_state(fsm); // execute immediately fp@251: return; fp@251: } fp@251: fp@251: // configure sync managers fp@293: ec_datagram_npwr(datagram, slave->station_address, 0x0800, fp@293: EC_SYNC_SIZE * slave->base_sync_count); fp@293: memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); fp@251: fp@325: list_for_each_entry(sync, &slave->sii_syncs, list) { fp@325: if (sync->index >= slave->base_sync_count) { fp@325: EC_ERR("Invalid sync manager configuration found!"); fp@325: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: ec_sync_config(sync, slave, fp@325: datagram->data + EC_SYNC_SIZE * sync->index); fp@251: } fp@251: fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@309: fsm->slave_state = ec_fsm_slaveconf_sync; fp@309: } fp@309: fp@309: /*****************************************************************************/ fp@309: fp@309: /** fp@309: Slave state: SYNC. fp@309: */ fp@309: fp@309: void ec_fsm_slaveconf_sync(ec_fsm_t *fsm /**< finite state machine */) fp@251: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@292: slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@260: EC_ERR("Failed to set sync managers on slave %i.\n", fp@251: slave->ring_position); fp@251: return; fp@251: } fp@251: fp@309: fsm->slave_state = ec_fsm_slaveconf_preop; fp@251: fsm->change_new = EC_SLAVE_STATE_PREOP; fp@251: fsm->change_state = ec_fsm_change_start; fp@251: fsm->change_state(fsm); // execute immediately fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@251: fp@251: /** fp@309: Slave state: PREOP. fp@309: */ fp@309: fp@309: void ec_fsm_slaveconf_preop(ec_fsm_t *fsm /**< finite state machine */) fp@251: { fp@251: ec_slave_t *slave = fsm->slave; fp@251: ec_master_t *master = fsm->master; fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@251: unsigned int j; fp@251: fp@251: fsm->change_state(fsm); // execute state change state machine fp@251: fp@325: if (fsm->change_state == ec_fsm_error) { fp@292: slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (fsm->change_state != ec_fsm_end) return; fp@251: fp@251: // slave is now in PREOP fp@251: if (slave->current_state == slave->requested_state) { fp@325: fsm->slave_state = ec_fsm_end; // successful fp@251: return; fp@251: } fp@251: fp@338: if (!slave->base_fmmu_count) { // skip FMMU configuration fp@338: if (list_empty(&slave->sdo_confs)) { // skip SDO configuration fp@338: fsm->slave_state = ec_fsm_slaveconf_saveop; fp@338: fsm->change_new = EC_SLAVE_STATE_SAVEOP; fp@338: fsm->change_state = ec_fsm_change_start; fp@338: fsm->change_state(fsm); // execute immediately fp@338: return; fp@338: } fp@338: fsm->slave_state = ec_fsm_slaveconf_sdoconf; fp@405: fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); fp@338: fsm->coe_state = ec_fsm_coe_down_start; fp@338: fsm->coe_state(fsm); // execute immediately fp@251: return; fp@251: } fp@251: fp@251: // configure FMMUs fp@293: ec_datagram_npwr(datagram, slave->station_address, fp@293: 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); fp@293: memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); fp@251: for (j = 0; j < slave->fmmu_count; j++) { fp@275: ec_fmmu_config(&slave->fmmus[j], slave, fp@293: datagram->data + EC_FMMU_SIZE * j); fp@293: } fp@293: fp@293: ec_master_queue_datagram(master, datagram); fp@309: fsm->slave_state = ec_fsm_slaveconf_fmmu; fp@309: } fp@309: fp@309: /*****************************************************************************/ fp@309: fp@309: /** fp@309: Slave state: FMMU. fp@309: */ fp@309: fp@309: void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */) fp@309: { fp@309: ec_datagram_t *datagram = &fsm->datagram; fp@329: ec_slave_t *slave = fsm->slave; fp@309: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@291: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@292: EC_ERR("Failed to set FMMUs on slave %i.\n", fp@292: fsm->slave->ring_position); fp@251: return; fp@251: } fp@251: fp@329: // No CoE configuration to be applied? Jump to SAVEOP state. fp@338: if (list_empty(&slave->sdo_confs)) { // skip SDO configuration fp@329: // set state to SAVEOP fp@329: fsm->slave_state = ec_fsm_slaveconf_saveop; fp@329: fsm->change_new = EC_SLAVE_STATE_SAVEOP; fp@329: fsm->change_state = ec_fsm_change_start; fp@329: fsm->change_state(fsm); // execute immediately fp@329: return; fp@329: } fp@329: fp@329: fsm->slave_state = ec_fsm_slaveconf_sdoconf; fp@405: fsm->coe_sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); fp@329: fsm->coe_state = ec_fsm_coe_down_start; fp@329: fsm->coe_state(fsm); // execute immediately fp@329: } fp@329: fp@329: /*****************************************************************************/ fp@329: fp@329: /** fp@329: Slave state: SDOCONF. fp@329: */ fp@329: fp@329: void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */) fp@329: { fp@329: fsm->coe_state(fsm); // execute CoE state machine fp@329: fp@329: if (fsm->coe_state == ec_fsm_error) { fp@329: fsm->slave->error_flag = 1; fp@329: fsm->slave_state = ec_fsm_error; fp@329: return; fp@329: } fp@329: fp@329: if (fsm->coe_state != ec_fsm_end) return; fp@329: fp@329: // Another SDO to configure? fp@405: if (fsm->coe_sdodata->list.next != &fsm->slave->sdo_confs) { fp@405: fsm->coe_sdodata = list_entry(fsm->coe_sdodata->list.next, fp@405: ec_sdo_data_t, list); fp@329: fsm->coe_state = ec_fsm_coe_down_start; fp@329: fsm->coe_state(fsm); // execute immediately fp@329: return; fp@329: } fp@329: fp@329: // All SDOs are now configured. fp@329: fp@251: // set state to SAVEOP fp@309: fsm->slave_state = ec_fsm_slaveconf_saveop; fp@251: fsm->change_new = EC_SLAVE_STATE_SAVEOP; fp@251: fsm->change_state = ec_fsm_change_start; fp@251: fsm->change_state(fsm); // execute immediately fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@251: fp@251: /** fp@309: Slave state: SAVEOP. fp@309: */ fp@309: fp@309: void ec_fsm_slaveconf_saveop(ec_fsm_t *fsm /**< finite state machine */) fp@251: { fp@251: fsm->change_state(fsm); // execute state change state machine fp@251: fp@325: if (fsm->change_state == ec_fsm_error) { fp@292: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (fsm->change_state != ec_fsm_end) return; fp@251: fp@251: // slave is now in SAVEOP fp@251: if (fsm->slave->current_state == fsm->slave->requested_state) { fp@325: fsm->slave_state = ec_fsm_end; // successful fp@251: return; fp@251: } fp@251: fp@251: // set state to OP fp@309: fsm->slave_state = ec_fsm_slaveconf_op; fp@251: fsm->change_new = EC_SLAVE_STATE_OP; fp@251: fsm->change_state = ec_fsm_change_start; fp@251: fsm->change_state(fsm); // execute immediately fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@251: fp@251: /** fp@309: Slave state: OP fp@309: */ fp@309: fp@309: void ec_fsm_slaveconf_op(ec_fsm_t *fsm /**< finite state machine */) fp@251: { fp@251: fsm->change_state(fsm); // execute state change state machine fp@251: fp@325: if (fsm->change_state == ec_fsm_error) { fp@292: fsm->slave->error_flag = 1; fp@325: fsm->slave_state = ec_fsm_error; fp@325: return; fp@325: } fp@325: fp@325: if (fsm->change_state != ec_fsm_end) return; fp@251: fp@251: // slave is now in OP fp@325: fsm->slave_state = ec_fsm_end; // successful fp@238: } fp@238: fp@238: /****************************************************************************** fp@325: * SII state machine fp@238: *****************************************************************************/ fp@238: fp@238: /** fp@260: SII state: START_READING. fp@238: Starts reading the slave information interface. fp@238: */ fp@238: fp@260: void ec_fsm_sii_start_reading(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@238: fp@238: // initiate read operation fp@260: if (fsm->sii_mode) { fp@293: ec_datagram_npwr(datagram, fsm->slave->station_address, 0x502, 4); fp@260: } fp@260: else { fp@293: ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4); fp@293: } fp@293: fp@293: EC_WRITE_U8 (datagram->data, 0x00); // read-only access fp@293: EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation fp@293: EC_WRITE_U16(datagram->data + 2, fsm->sii_offset); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@269: fsm->sii_state = ec_fsm_sii_read_check; fp@269: } fp@269: fp@269: /*****************************************************************************/ fp@269: fp@269: /** fp@269: SII state: READ_CHECK. fp@293: Checks, if the SII-read-datagram has been sent and issues a fetch datagram. fp@238: */ fp@238: fp@269: void ec_fsm_sii_read_check(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@293: EC_ERR("SII: Reception of read datagram failed.\n"); fp@325: fsm->sii_state = ec_fsm_error; fp@238: return; fp@238: } fp@238: fp@398: fsm->sii_start = datagram->cycles_sent; fp@397: fsm->sii_check_once_more = 1; fp@260: fp@293: // issue check/fetch datagram fp@260: if (fsm->sii_mode) { fp@293: ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); fp@260: } fp@260: else { fp@293: ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10); fp@293: } fp@293: fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@269: fsm->sii_state = ec_fsm_sii_read_fetch; fp@269: } fp@269: fp@269: /*****************************************************************************/ fp@269: fp@269: /** fp@269: SII state: READ_FETCH. fp@293: Fetches the result of an SII-read datagram. fp@238: */ fp@238: fp@269: void ec_fsm_sii_read_fetch(ec_fsm_t *fsm /**< finite state machine */) fp@238: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@293: EC_ERR("SII: Reception of check/fetch datagram failed.\n"); fp@325: fsm->sii_state = ec_fsm_error; fp@238: return; fp@238: } fp@238: fp@238: // check "busy bit" fp@293: if (EC_READ_U8(datagram->data + 1) & 0x81) { fp@260: // still busy... timeout? fp@398: if (datagram->cycles_received fp@398: - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { fp@397: if (!fsm->sii_check_once_more) { fp@397: EC_ERR("SII: Read timeout.\n"); fp@397: fsm->sii_state = ec_fsm_error; fp@260: #if 0 fp@397: EC_DBG("SII busy: %02X %02X %02X %02X\n", fp@397: EC_READ_U8(datagram->data + 0), fp@397: EC_READ_U8(datagram->data + 1), fp@397: EC_READ_U8(datagram->data + 2), fp@397: EC_READ_U8(datagram->data + 3)); fp@260: #endif fp@397: return; fp@397: } fp@397: fsm->sii_check_once_more = 0; fp@260: } fp@260: fp@293: // issue check/fetch datagram again fp@260: if (fsm->sii_mode) { fp@293: ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); fp@260: } fp@260: else { fp@293: ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10); fp@260: } fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@260: return; fp@260: } fp@260: fp@260: #if 0 fp@260: EC_DBG("SII rec: %02X %02X %02X %02X - %02X %02X %02X %02X\n", fp@293: EC_READ_U8(datagram->data + 0), EC_READ_U8(datagram->data + 1), fp@293: EC_READ_U8(datagram->data + 2), EC_READ_U8(datagram->data + 3), fp@293: EC_READ_U8(datagram->data + 6), EC_READ_U8(datagram->data + 7), fp@293: EC_READ_U8(datagram->data + 8), EC_READ_U8(datagram->data + 9)); fp@260: #endif fp@260: fp@260: // SII value received. fp@293: memcpy(fsm->sii_value, datagram->data + 6, 4); fp@325: fsm->sii_state = ec_fsm_end; fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@269: SII state: START_WRITING. fp@269: Starts reading the slave information interface. fp@269: */ fp@269: fp@269: void ec_fsm_sii_start_writing(ec_fsm_t *fsm /**< finite state machine */) fp@269: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@269: fp@269: // initiate write operation fp@293: ec_datagram_npwr(datagram, fsm->slave->station_address, 0x502, 8); fp@293: EC_WRITE_U8 (datagram->data, 0x01); // enable write access fp@293: EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation fp@293: EC_WRITE_U32(datagram->data + 2, fsm->sii_offset); fp@293: memcpy(datagram->data + 6, fsm->sii_value, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@269: fsm->sii_state = ec_fsm_sii_write_check; fp@269: } fp@269: fp@269: /*****************************************************************************/ fp@269: fp@269: /** fp@269: SII state: WRITE_CHECK. fp@269: */ fp@269: fp@269: void ec_fsm_sii_write_check(ec_fsm_t *fsm /**< finite state machine */) fp@269: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@293: EC_ERR("SII: Reception of write datagram failed.\n"); fp@325: fsm->sii_state = ec_fsm_error; fp@269: return; fp@269: } fp@269: fp@398: fsm->sii_start = datagram->cycles_sent; fp@398: fsm->sii_check_once_more = 1; fp@269: fp@293: // issue check/fetch datagram fp@293: ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@269: fsm->sii_state = ec_fsm_sii_write_check2; fp@269: } fp@269: fp@269: /*****************************************************************************/ fp@269: fp@269: /** fp@269: SII state: WRITE_CHECK2. fp@269: */ fp@269: fp@269: void ec_fsm_sii_write_check2(ec_fsm_t *fsm /**< finite state machine */) fp@269: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@293: EC_ERR("SII: Reception of write check datagram failed.\n"); fp@325: fsm->sii_state = ec_fsm_error; fp@269: return; fp@269: } fp@269: fp@293: if (EC_READ_U8(datagram->data + 1) & 0x82) { fp@269: // still busy... timeout? fp@398: if (datagram->cycles_received fp@398: - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { fp@398: if (!fsm->sii_check_once_more) { fp@398: EC_ERR("SII: Write timeout.\n"); fp@398: fsm->sii_state = ec_fsm_error; fp@398: return; fp@398: } fp@398: fsm->sii_check_once_more = 0; fp@269: } fp@269: fp@293: // issue check/fetch datagram again fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@395: return; fp@395: } fp@395: fp@395: if (EC_READ_U8(datagram->data + 1) & 0x40) { fp@269: EC_ERR("SII: Write operation failed!\n"); fp@325: fsm->sii_state = ec_fsm_error; fp@395: return; fp@395: } fp@395: fp@395: // success fp@395: fsm->sii_state = ec_fsm_end; fp@238: } fp@238: fp@251: /****************************************************************************** fp@325: * state change state machine fp@251: *****************************************************************************/ fp@251: fp@251: /** fp@260: Change state: START. fp@260: */ fp@260: fp@260: void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */) fp@251: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@251: ec_slave_t *slave = fsm->slave; fp@251: fp@398: fsm->change_take_time = 1; fp@325: fp@251: // write new state to slave fp@293: ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); fp@293: EC_WRITE_U16(datagram->data, fsm->change_new); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@251: fsm->change_state = ec_fsm_change_check; fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@251: fp@251: /** fp@260: Change state: CHECK. fp@260: */ fp@260: fp@260: void ec_fsm_change_check(ec_fsm_t *fsm /**< finite state machine */) fp@260: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@325: fsm->change_state = ec_fsm_error; fp@293: EC_ERR("Failed to send state datagram to slave %i!\n", fp@260: fsm->slave->ring_position); fp@292: return; fp@292: } fp@292: fp@398: if (fsm->change_take_time) { fp@398: fsm->change_take_time = 0; fp@398: fsm->change_jiffies = datagram->jiffies_sent; fp@398: } fp@398: fp@293: if (datagram->working_counter != 1) { fp@398: if (datagram->jiffies_received - fsm->change_jiffies >= 3 * HZ) { fp@325: fsm->change_state = ec_fsm_error; fp@325: EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" fp@325: " respond.\n", fsm->change_new, fsm->slave->ring_position); fp@325: return; fp@325: } fp@325: fp@325: // repeat writing new state to slave fp@325: ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); fp@325: EC_WRITE_U16(datagram->data, fsm->change_new); fp@325: ec_master_queue_datagram(fsm->master, datagram); fp@260: return; fp@260: } fp@260: fp@398: fsm->change_take_time = 1; fp@251: fp@251: // read AL status from slave fp@293: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@251: fsm->change_state = ec_fsm_change_status; fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@251: fp@251: /** fp@260: Change state: STATUS. fp@260: */ fp@260: fp@260: void ec_fsm_change_status(ec_fsm_t *fsm /**< finite state machine */) fp@251: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@325: fsm->change_state = ec_fsm_error; fp@260: EC_ERR("Failed to check state 0x%02X on slave %i.\n", fp@260: fsm->change_new, slave->ring_position); fp@251: return; fp@251: } fp@251: fp@398: if (fsm->change_take_time) { fp@398: fsm->change_take_time = 0; fp@398: fsm->change_jiffies = datagram->jiffies_sent; fp@398: } fp@398: fp@293: slave->current_state = EC_READ_U8(datagram->data); fp@251: fp@260: if (slave->current_state == fsm->change_new) { fp@260: // state has been set successfully fp@325: fsm->change_state = ec_fsm_end; fp@260: return; fp@260: } fp@260: fp@402: if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { fp@260: // state change error fp@260: fsm->change_new = slave->current_state & 0x0F; fp@251: EC_ERR("Failed to set state 0x%02X - Slave %i refused state change" fp@251: " (code 0x%02X)!\n", fsm->change_new, slave->ring_position, fp@251: slave->current_state); fp@251: // fetch AL status error code fp@293: ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@251: fsm->change_state = ec_fsm_change_code; fp@251: return; fp@251: } fp@251: fp@398: if (datagram->jiffies_received fp@398: - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms fp@260: // timeout while checking fp@325: fsm->change_state = ec_fsm_error; fp@260: EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", fp@260: fsm->change_new, slave->ring_position); fp@260: return; fp@260: } fp@260: fp@260: // still old state: check again fp@293: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@260: } fp@260: fp@260: /*****************************************************************************/ fp@260: fp@260: /** fp@251: Application layer status messages. fp@251: */ fp@251: fp@251: const ec_code_msg_t al_status_messages[] = { fp@251: {0x0001, "Unspecified error"}, fp@251: {0x0011, "Invalud requested state change"}, fp@251: {0x0012, "Unknown requested state"}, fp@251: {0x0013, "Bootstrap not supported"}, fp@251: {0x0014, "No valid firmware"}, fp@251: {0x0015, "Invalid mailbox configuration"}, fp@251: {0x0016, "Invalid mailbox configuration"}, fp@251: {0x0017, "Invalid sync manager configuration"}, fp@251: {0x0018, "No valid inputs available"}, fp@251: {0x0019, "No valid outputs"}, fp@251: {0x001A, "Synchronisation error"}, fp@251: {0x001B, "Sync manager watchdog"}, fp@297: {0x001C, "Invalid sync manager types"}, fp@297: {0x001D, "Invalid output configuration"}, fp@297: {0x001E, "Invalid input configuration"}, fp@297: {0x001F, "Invalid watchdog configuration"}, fp@251: {0x0020, "Slave needs cold start"}, fp@251: {0x0021, "Slave needs INIT"}, fp@251: {0x0022, "Slave needs PREOP"}, fp@251: {0x0023, "Slave needs SAVEOP"}, fp@297: {0x0030, "Invalid DC SYNCH configuration"}, fp@297: {0x0031, "Invalid DC latch configuration"}, fp@297: {0x0032, "PLL error"}, fp@297: {0x0033, "Invalid DC IO error"}, fp@297: {0x0034, "Invalid DC timeout error"}, fp@297: {0x0042, "MBOX EOE"}, fp@297: {0x0043, "MBOX COE"}, fp@297: {0x0044, "MBOX FOE"}, fp@297: {0x0045, "MBOX SOE"}, fp@297: {0x004F, "MBOX VOE"}, fp@251: {} fp@251: }; fp@251: fp@251: /*****************************************************************************/ fp@286: fp@286: /** fp@286: Change state: CODE. fp@286: */ fp@286: fp@286: void ec_fsm_change_code(ec_fsm_t *fsm /**< finite state machine */) fp@286: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@286: ec_slave_t *slave = fsm->slave; fp@286: uint32_t code; fp@286: const ec_code_msg_t *al_msg; fp@286: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@325: fsm->change_state = ec_fsm_error; fp@293: EC_ERR("Reception of AL status code datagram failed.\n"); fp@293: return; fp@293: } fp@293: fp@293: if ((code = EC_READ_U16(datagram->data))) { fp@286: for (al_msg = al_status_messages; al_msg->code; al_msg++) { fp@286: if (al_msg->code != code) continue; fp@286: EC_ERR("AL status message 0x%04X: \"%s\".\n", fp@286: al_msg->code, al_msg->message); fp@286: break; fp@286: } fp@286: if (!al_msg->code) fp@286: EC_ERR("Unknown AL status code 0x%04X.\n", code); fp@286: } fp@286: fp@286: // acknowledge "old" slave state fp@293: ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); fp@293: EC_WRITE_U16(datagram->data, slave->current_state); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@286: fsm->change_state = ec_fsm_change_ack; fp@286: } fp@286: fp@286: /*****************************************************************************/ fp@286: fp@286: /** fp@286: Change state: ACK. fp@286: */ fp@286: fp@286: void ec_fsm_change_ack(ec_fsm_t *fsm /**< finite state machine */) fp@286: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@325: fsm->change_state = ec_fsm_error; fp@293: EC_ERR("Reception of state ack datagram failed.\n"); fp@286: return; fp@286: } fp@286: fp@398: fsm->change_take_time = 1; fp@301: fp@286: // read new AL status fp@293: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@293: ec_master_queue_datagram(fsm->master, datagram); fp@311: fsm->change_state = ec_fsm_change_check_ack; fp@311: } fp@311: fp@311: /*****************************************************************************/ fp@311: fp@311: /** fp@311: Change state: CHECK ACK. fp@311: */ fp@311: fp@311: void ec_fsm_change_check_ack(ec_fsm_t *fsm /**< finite state machine */) fp@286: { fp@293: ec_datagram_t *datagram = &fsm->datagram; fp@293: ec_slave_t *slave = fsm->slave; fp@301: ec_slave_state_t ack_state; fp@293: fp@325: if (datagram->state != EC_DATAGRAM_RECEIVED fp@325: || datagram->working_counter != 1) { fp@325: fsm->change_state = ec_fsm_error; fp@293: EC_ERR("Reception of state ack check datagram failed.\n"); fp@293: return; fp@293: } fp@293: fp@398: if (fsm->change_take_time) { fp@398: fsm->change_take_time = 0; fp@398: fsm->change_jiffies = datagram->jiffies_sent; fp@398: } fp@398: fp@301: ack_state = EC_READ_U8(datagram->data); fp@301: fp@301: if (ack_state == slave->current_state) { fp@325: fsm->change_state = ec_fsm_error; fp@286: EC_INFO("Acknowleged state 0x%02X on slave %i.\n", fp@286: slave->current_state, slave->ring_position); fp@292: return; fp@292: } fp@292: fp@398: if (datagram->jiffies_received fp@398: - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms fp@301: // timeout while checking fp@301: slave->current_state = EC_SLAVE_STATE_UNKNOWN; fp@325: fsm->change_state = ec_fsm_error; fp@301: EC_ERR("Timeout while acknowleging state 0x%02X on slave %i.\n", fp@301: fsm->change_new, slave->ring_position); fp@301: return; fp@301: } fp@301: fp@301: // reread new AL status fp@301: ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); fp@301: ec_master_queue_datagram(fsm->master, datagram); fp@286: } fp@286: fp@329: /****************************************************************************** fp@329: * CoE state machine fp@329: *****************************************************************************/ fp@329: fp@329: /** fp@329: CoE state: DOWN_START. fp@329: */ fp@329: fp@329: void ec_fsm_coe_down_start(ec_fsm_t *fsm /**< finite state machine */) fp@329: { fp@329: ec_datagram_t *datagram = &fsm->datagram; fp@329: ec_slave_t *slave = fsm->slave; fp@405: ec_sdo_data_t *sdodata = fsm->coe_sdodata; fp@329: uint8_t *data; fp@329: fp@329: EC_INFO("Downloading SDO 0x%04X:%i to slave %i.\n", fp@329: sdodata->index, sdodata->subindex, slave->ring_position); fp@329: fp@329: if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) { fp@329: EC_ERR("SDO fragmenting not supported yet!\n"); fp@329: fsm->coe_state = ec_fsm_error; fp@329: return; fp@329: } fp@329: fp@329: if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, fp@329: sdodata->size + 10))) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: return; fp@329: } fp@329: fp@329: EC_WRITE_U16(data, 0x2 << 12); // SDO request fp@329: EC_WRITE_U8 (data + 2, (0x1 // size specified fp@329: | 0x1 << 5)); // Download request fp@329: EC_WRITE_U16(data + 3, sdodata->index); fp@329: EC_WRITE_U8 (data + 5, sdodata->subindex); fp@329: EC_WRITE_U32(data + 6, sdodata->size); fp@390: memcpy(data + 10, sdodata->data, sdodata->size); fp@329: fp@329: ec_master_queue_datagram(fsm->master, datagram); fp@329: fsm->coe_state = ec_fsm_coe_down_request; fp@329: } fp@329: fp@329: /*****************************************************************************/ fp@329: fp@329: /** fp@329: CoE state: DOWN_REQUEST. fp@329: */ fp@329: fp@329: void ec_fsm_coe_down_request(ec_fsm_t *fsm /**< finite state machine */) fp@329: { fp@329: ec_datagram_t *datagram = &fsm->datagram; fp@329: ec_slave_t *slave = fsm->slave; fp@329: fp@329: if (datagram->state != EC_DATAGRAM_RECEIVED fp@329: || datagram->working_counter != 1) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("Reception of CoE download request failed.\n"); fp@329: return; fp@329: } fp@329: fp@398: fsm->coe_start = datagram->cycles_sent; fp@329: fp@329: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@329: ec_master_queue_datagram(fsm->master, datagram); fp@329: fsm->coe_state = ec_fsm_coe_down_check; fp@329: } fp@329: fp@329: /*****************************************************************************/ fp@329: fp@329: /** fp@329: CoE state: DOWN_CHECK. fp@329: */ fp@329: fp@329: void ec_fsm_coe_down_check(ec_fsm_t *fsm /**< finite state machine */) fp@329: { fp@329: ec_datagram_t *datagram = &fsm->datagram; fp@329: ec_slave_t *slave = fsm->slave; fp@329: fp@329: if (datagram->state != EC_DATAGRAM_RECEIVED fp@329: || datagram->working_counter != 1) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("Reception of CoE mailbox check datagram failed.\n"); fp@329: return; fp@329: } fp@329: fp@329: if (!ec_slave_mbox_check(datagram)) { fp@398: if (datagram->cycles_received fp@398: - fsm->coe_start >= (cycles_t) 100 * cpu_khz) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("Timeout while checking SDO configuration on slave %i.\n", fp@329: slave->ring_position); fp@329: return; fp@329: } fp@329: fp@329: ec_slave_mbox_prepare_check(slave, datagram); // can not fail. fp@329: ec_master_queue_datagram(fsm->master, datagram); fp@329: return; fp@329: } fp@329: fp@329: // Fetch response fp@329: ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. fp@329: ec_master_queue_datagram(fsm->master, datagram); fp@339: fsm->coe_state = ec_fsm_coe_down_response; fp@339: } fp@339: fp@339: /*****************************************************************************/ fp@339: fp@339: /** fp@339: CoE state: DOWN_RESPONSE. fp@339: */ fp@339: fp@339: void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */) fp@329: { fp@329: ec_datagram_t *datagram = &fsm->datagram; fp@329: ec_slave_t *slave = fsm->slave; fp@391: uint8_t *data, mbox_prot; fp@329: size_t rec_size; fp@405: ec_sdo_data_t *sdodata = fsm->coe_sdodata; fp@329: fp@329: if (datagram->state != EC_DATAGRAM_RECEIVED fp@329: || datagram->working_counter != 1) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("Reception of CoE download response failed.\n"); fp@329: return; fp@329: } fp@329: fp@390: if (!(data = ec_slave_mbox_fetch(slave, datagram, fp@391: &mbox_prot, &rec_size))) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: return; fp@329: } fp@329: fp@391: if (mbox_prot != 0x03) { // CoE fp@391: EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); fp@391: fsm->coe_state = ec_fsm_error; fp@390: return; fp@390: } fp@390: fp@329: if (rec_size < 6) { fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("Received data is too small (%i bytes):\n", rec_size); fp@329: ec_print_data(data, rec_size); fp@329: return; fp@329: } fp@329: fp@329: if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request fp@329: EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n", fp@329: sdodata->index, sdodata->subindex, sdodata->size, fp@329: slave->ring_position); fp@329: if (rec_size < 10) { fp@329: EC_ERR("Incomplete Abort command:\n"); fp@329: ec_print_data(data, rec_size); fp@329: } fp@329: else fp@329: ec_canopen_abort_msg(EC_READ_U32(data + 6)); fp@329: return; fp@329: } fp@329: fp@329: if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response fp@329: EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response fp@329: EC_READ_U16(data + 3) != sdodata->index || // index fp@329: EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex fp@329: fsm->coe_state = ec_fsm_error; fp@329: EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n", fp@329: sdodata->index, sdodata->subindex, sdodata->size); fp@329: EC_ERR("Invalid SDO download response at slave %i!\n", fp@329: slave->ring_position); fp@329: ec_print_data(data, rec_size); fp@329: return; fp@329: } fp@329: fp@329: fsm->coe_state = ec_fsm_end; // success fp@329: } fp@329: fp@329: /*****************************************************************************/ fp@329: fp@329: /** fp@329: SDO abort messages. fp@329: The "abort SDO transfer request" supplies an abort code, fp@329: which can be translated to clear text. This table does fp@329: the mapping of the codes and messages. fp@329: */ fp@329: fp@329: const ec_code_msg_t sdo_abort_messages[] = { fp@329: {0x05030000, "Toggle bit not changed"}, fp@329: {0x05040000, "SDO protocol timeout"}, fp@329: {0x05040001, "Client/Server command specifier not valid or unknown"}, fp@329: {0x05040005, "Out of memory"}, fp@329: {0x06010000, "Unsupported access to an object"}, fp@329: {0x06010001, "Attempt to read a write-only object"}, fp@329: {0x06010002, "Attempt to write a read-only object"}, fp@329: {0x06020000, "This object does not exist in the object directory"}, fp@329: {0x06040041, "The object cannot be mapped into the PDO"}, fp@329: {0x06040042, "The number and length of the objects to be mapped would" fp@329: " exceed the PDO length"}, fp@329: {0x06040043, "General parameter incompatibility reason"}, fp@329: {0x06040047, "Gerneral internal incompatibility in device"}, fp@329: {0x06060000, "Access failure due to a hardware error"}, fp@329: {0x06070010, "Data type does not match, length of service parameter does" fp@329: " not match"}, fp@329: {0x06070012, "Data type does not match, length of service parameter too" fp@329: " high"}, fp@329: {0x06070013, "Data type does not match, length of service parameter too" fp@329: " low"}, fp@329: {0x06090011, "Subindex does not exist"}, fp@329: {0x06090030, "Value range of parameter exceeded"}, fp@329: {0x06090031, "Value of parameter written too high"}, fp@329: {0x06090032, "Value of parameter written too low"}, fp@329: {0x06090036, "Maximum value is less than minimum value"}, fp@329: {0x08000000, "General error"}, fp@329: {0x08000020, "Data cannot be transferred or stored to the application"}, fp@329: {0x08000021, "Data cannot be transferred or stored to the application" fp@329: " because of local control"}, fp@329: {0x08000022, "Data cannot be transferred or stored to the application" fp@329: " because of the present device state"}, fp@329: {0x08000023, "Object dictionary dynamic generation fails or no object" fp@329: " dictionary is present"}, fp@329: {} fp@329: }; fp@329: fp@329: /*****************************************************************************/ fp@329: fp@329: /** fp@329: Outputs an SDO abort message. fp@329: */ fp@329: fp@329: void ec_canopen_abort_msg(uint32_t abort_code) fp@329: { fp@329: const ec_code_msg_t *abort_msg; fp@329: fp@329: for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { fp@329: if (abort_msg->code == abort_code) { fp@329: EC_ERR("SDO abort message 0x%08X: \"%s\".\n", fp@329: abort_msg->code, abort_msg->message); fp@329: return; fp@329: } fp@329: } fp@329: fp@329: EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); fp@329: } fp@329: fp@329: /****************************************************************************** fp@329: * Common state functions fp@329: *****************************************************************************/ fp@286: fp@286: /** fp@325: State: ERROR. fp@325: */ fp@325: fp@325: void ec_fsm_error(ec_fsm_t *fsm /**< finite state machine */) fp@325: { fp@325: } fp@325: fp@325: /*****************************************************************************/ fp@325: fp@325: /** fp@325: State: END. fp@325: */ fp@325: fp@325: void ec_fsm_end(ec_fsm_t *fsm /**< finite state machine */) fp@325: { fp@325: } fp@325: fp@325: /*****************************************************************************/