fp@830: /****************************************************************************** fp@830: * fp@830: * $Id$ fp@830: * fp@1326: * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH fp@830: * fp@830: * This file is part of the IgH EtherCAT Master. fp@830: * fp@1326: * The IgH EtherCAT Master is free software; you can redistribute it and/or fp@1326: * modify it under the terms of the GNU General Public License version 2, as fp@1326: * published by the Free Software Foundation. fp@1326: * fp@1326: * The IgH EtherCAT Master is distributed in the hope that it will be useful, fp@1326: * but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1326: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General fp@1326: * Public License for more details. fp@1326: * fp@1326: * You should have received a copy of the GNU General Public License along fp@1326: * with the IgH EtherCAT Master; if not, write to the Free Software fp@830: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@830: * fp@1363: * --- fp@1363: * fp@1363: * The license mentioned above concerns the source code only. Using the fp@1363: * EtherCAT technology and brand is only permitted in compliance with the fp@1363: * industrial property and similar rights of Beckhoff Automation GmbH. fp@830: * fp@830: *****************************************************************************/ fp@830: fp@1064: /** \file fp@1064: * fp@1064: * EtherCAT slave configuration state machine. fp@1064: */ fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1436: #include fp@1436: fp@830: #include "globals.h" fp@830: #include "master.h" fp@830: #include "mailbox.h" fp@830: #include "slave_config.h" fp@830: #include "fsm_slave_config.h" fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1925: /** Maximum clock difference (in ns) before going to SAFEOP. fp@1925: * fp@1925: * Wait for DC time difference to drop under this absolute value before fp@1925: * requesting SAFEOP. fp@1925: */ fp@1925: #define EC_DC_MAX_SYNC_DIFF_NS 5000 fp@1925: fp@1925: /** Maximum time (in ms) to wait for clock discipline. fp@1925: */ fp@1925: #define EC_DC_SYNC_WAIT_MS 5000 fp@1925: fp@1925: /** Time offset (in ns), that is added to cyclic start time. fp@1925: */ fp@1925: #define EC_DC_START_OFFSET 100000000ULL fp@1889: fp@1889: /*****************************************************************************/ fp@1889: fp@830: void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); fp@1342: void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); fp@1407: void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); fp@1337: void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); fp@1944: void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *); fp@1509: void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); fp@1509: void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); fp@888: void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); fp@1174: void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); fp@1392: void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); fp@1925: void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *); fp@1392: void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); fp@1392: void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); fp@1944: void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); fp@830: fp@1318: void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); fp@1342: void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); fp@1407: void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); fp@1337: void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); fp@1944: void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *); fp@1180: void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); fp@1509: void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *); fp@1509: void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *); fp@888: void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); fp@1407: void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); fp@1944: void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *); fp@1944: void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *); fp@830: fp@830: void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); fp@830: void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *); fp@830: fp@1318: void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *); fp@1318: fp@830: /*****************************************************************************/ fp@830: fp@830: /** Constructor. fp@830: */ fp@1064: void ec_fsm_slave_config_init( fp@1064: ec_fsm_slave_config_t *fsm, /**< slave state machine */ fp@1174: ec_datagram_t *datagram, /**< datagram structure to use */ fp@1174: ec_fsm_change_t *fsm_change, /**< State change state machine to use. */ fp@1174: ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */ fp@1327: ec_fsm_pdo_t *fsm_pdo /**< PDO configuration state machine to use. */ fp@830: ) fp@830: { fp@1318: ec_sdo_request_init(&fsm->request_copy); fp@1849: ec_soe_request_init(&fsm->soe_request_copy); fp@1318: fp@830: fsm->datagram = datagram; fp@1174: fsm->fsm_change = fsm_change; fp@1174: fsm->fsm_coe = fsm_coe; fp@1174: fsm->fsm_pdo = fsm_pdo; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@830: /** Destructor. fp@830: */ fp@1064: void ec_fsm_slave_config_clear( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@1318: ec_sdo_request_clear(&fsm->request_copy); fp@1844: ec_soe_request_clear(&fsm->soe_request_copy); fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Start slave configuration state machine. fp@1064: */ fp@1064: void ec_fsm_slave_config_start( fp@1064: ec_fsm_slave_config_t *fsm, /**< slave state machine */ fp@830: ec_slave_t *slave /**< slave to configure */ fp@830: ) fp@830: { fp@830: fsm->slave = slave; fp@830: fsm->state = ec_fsm_slave_config_state_start; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@830: /** fp@1064: * \return false, if state machine has terminated fp@1064: */ fp@1064: int ec_fsm_slave_config_running( fp@1064: const ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: return fsm->state != ec_fsm_slave_config_state_end fp@830: && fsm->state != ec_fsm_slave_config_state_error; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Executes the current state of the state machine. fp@1064: * fp@1064: * If the state machine's datagram is not sent or received yet, the execution fp@1064: * of the state machine is delayed to the next cycle. fp@1064: * fp@1064: * \return false, if state machine has terminated fp@1064: */ fp@1064: int ec_fsm_slave_config_exec( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: if (fsm->datagram->state == EC_DATAGRAM_SENT fp@830: || fsm->datagram->state == EC_DATAGRAM_QUEUED) { fp@830: // datagram was not sent or received yet. fp@830: return ec_fsm_slave_config_running(fsm); fp@830: } fp@830: fp@830: fsm->state(fsm); fp@830: return ec_fsm_slave_config_running(fsm); fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@830: /** fp@1064: * \return true, if the state machine terminated gracefully fp@1064: */ fp@1064: int ec_fsm_slave_config_success( fp@1064: const ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: return fsm->state == ec_fsm_slave_config_state_end; fp@830: } fp@830: fp@830: /****************************************************************************** fp@830: * Slave configuration state machine fp@830: *****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: START. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_start( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@1921: EC_SLAVE_DBG(fsm->slave, 1, "Configuring...\n"); fp@1318: ec_fsm_slave_config_enter_init(fsm); fp@1318: } fp@1318: fp@1318: /*****************************************************************************/ fp@1318: fp@1318: /** Start state change to INIT. fp@1318: */ fp@1318: void ec_fsm_slave_config_enter_init( fp@1318: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1318: ) fp@1318: { fp@1174: ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); fp@1174: ec_fsm_change_exec(fsm->fsm_change); fp@830: fsm->state = ec_fsm_slave_config_state_init; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: INIT. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_init( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_slave_t *slave = fsm->slave; fp@830: ec_datagram_t *datagram = fsm->datagram; fp@830: fp@1174: if (ec_fsm_change_exec(fsm->fsm_change)) return; fp@1174: fp@1174: if (!ec_fsm_change_success(fsm->fsm_change)) { fp@1174: if (!fsm->fsm_change->spontaneous_change) fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@830: return; fp@830: } fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Now in INIT.\n"); fp@830: fp@830: if (!slave->base_fmmu_count) { // skip FMMU configuration fp@1342: ec_fsm_slave_config_enter_clear_sync(fsm); fp@830: return; fp@830: } fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Clearing FMMU configurations...\n"); fp@830: fp@830: // clear FMMU configurations fp@830: ec_datagram_fpwr(datagram, slave->station_address, fp@830: 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); fp@1225: ec_datagram_zero(datagram); fp@830: fsm->retries = EC_FSM_RETRIES; fp@830: fsm->state = ec_fsm_slave_config_state_clear_fmmus; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: CLEAR FMMU. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_clear_fmmus( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_datagram_t *datagram = fsm->datagram; fp@830: fp@830: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@830: return; fp@830: fp@830: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(fsm->slave, "Failed receive FMMU clearing datagram.\n"); fp@830: return; fp@830: } fp@830: fp@830: if (datagram->working_counter != 1) { fp@830: fsm->slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(fsm->slave, "Failed to clear FMMUs: "); fp@830: ec_datagram_print_wc_error(datagram); fp@830: return; fp@830: } fp@830: fp@1342: ec_fsm_slave_config_enter_clear_sync(fsm); fp@1342: } fp@1342: fp@1342: /*****************************************************************************/ fp@1342: fp@1342: /** Clear the sync manager configurations. fp@1342: */ fp@1342: void ec_fsm_slave_config_enter_clear_sync( fp@1342: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1342: ) fp@1342: { fp@1342: ec_slave_t *slave = fsm->slave; fp@1342: ec_datagram_t *datagram = fsm->datagram; fp@1342: size_t sync_size; fp@1342: fp@1471: if (!slave->base_sync_count) { fp@1404: // no sync managers fp@1407: ec_fsm_slave_config_enter_dc_clear_assign(fsm); fp@1342: return; fp@1342: } fp@1342: fp@1921: EC_SLAVE_DBG(slave, 1, "Clearing sync manager configurations...\n"); fp@1344: fp@1471: sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count; fp@1342: fp@1342: // clear sync manager configurations fp@1342: ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size); fp@1896: ec_datagram_zero(datagram); fp@1342: fsm->retries = EC_FSM_RETRIES; fp@1342: fsm->state = ec_fsm_slave_config_state_clear_sync; fp@1342: } fp@1342: fp@1342: /*****************************************************************************/ fp@1342: fp@1342: /** Slave configuration state: CLEAR SYNC. fp@1342: */ fp@1342: void ec_fsm_slave_config_state_clear_sync( fp@1342: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1342: ) fp@1342: { fp@1342: ec_datagram_t *datagram = fsm->datagram; fp@1342: fp@1342: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1342: return; fp@1342: fp@1342: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1342: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(fsm->slave, "Failed receive sync manager" fp@1921: " clearing datagram.\n"); fp@1342: return; fp@1342: } fp@1342: fp@1342: if (datagram->working_counter != 1) { fp@1342: fsm->slave->error_flag = 1; fp@1342: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(fsm->slave, fp@1921: "Failed to clear sync manager configurations: "); fp@1342: ec_datagram_print_wc_error(datagram); fp@1342: return; fp@1342: } fp@1342: fp@1407: ec_fsm_slave_config_enter_dc_clear_assign(fsm); fp@1406: } fp@1406: fp@1406: /*****************************************************************************/ fp@1406: fp@1406: /** Clear the DC assignment. fp@1406: */ fp@1407: void ec_fsm_slave_config_enter_dc_clear_assign( fp@1406: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1406: ) fp@1406: { fp@1406: ec_slave_t *slave = fsm->slave; fp@1406: ec_datagram_t *datagram = fsm->datagram; fp@1406: fp@1419: if (!slave->base_dc_supported || !slave->has_dc_system_time) { fp@1406: ec_fsm_slave_config_enter_mbox_sync(fsm); fp@1406: return; fp@1406: } fp@1406: fp@1921: EC_SLAVE_DBG(slave, 1, "Clearing DC assignment...\n"); fp@1406: fp@1406: ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); fp@1406: ec_datagram_zero(datagram); fp@1406: fsm->retries = EC_FSM_RETRIES; fp@1407: fsm->state = ec_fsm_slave_config_state_dc_clear_assign; fp@1406: } fp@1406: fp@1406: /*****************************************************************************/ fp@1406: fp@1406: /** Slave configuration state: CLEAR DC ASSIGN. fp@1406: */ fp@1407: void ec_fsm_slave_config_state_dc_clear_assign( fp@1406: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1406: ) fp@1406: { fp@1406: ec_datagram_t *datagram = fsm->datagram; fp@1406: fp@1406: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1406: return; fp@1406: fp@1406: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1406: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment" fp@1921: " clearing datagram.\n"); fp@1921: return; fp@1921: } fp@1921: fp@1921: if (datagram->working_counter != 1) { fp@1407: // clearing the DC assignment does not succeed on simple slaves fp@1921: EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: "); fp@1407: ec_datagram_print_wc_error(datagram); fp@1407: } fp@1407: fp@830: ec_fsm_slave_config_enter_mbox_sync(fsm); fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Check for mailbox sync managers to be configured. fp@1064: */ fp@830: void ec_fsm_slave_config_enter_mbox_sync( fp@830: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@830: ) fp@830: { fp@830: ec_slave_t *slave = fsm->slave; fp@830: ec_datagram_t *datagram = fsm->datagram; fp@830: unsigned int i; fp@830: fp@830: // slave is now in INIT fp@830: if (slave->current_state == slave->requested_state) { fp@830: fsm->state = ec_fsm_slave_config_state_end; // successful fp@1921: EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); fp@830: return; fp@830: } fp@830: fp@834: if (!slave->sii.mailbox_protocols) { fp@830: // no mailbox protocols supported fp@1921: EC_SLAVE_DBG(slave, 1, "Slave does not support" fp@1921: " mailbox communication.\n"); fp@1337: ec_fsm_slave_config_enter_boot_preop(fsm); fp@830: return; fp@830: } fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n"); fp@830: fp@1337: if (slave->requested_state == EC_SLAVE_STATE_BOOT) { fp@1337: ec_sync_t sync; fp@1337: fp@1337: ec_datagram_fpwr(datagram, slave->station_address, 0x0800, fp@1337: EC_SYNC_PAGE_SIZE * 2); fp@1896: ec_datagram_zero(datagram); fp@1337: fp@1337: ec_sync_init(&sync, slave); fp@1337: sync.physical_start_address = slave->sii.boot_rx_mailbox_offset; fp@1337: sync.control_register = 0x26; fp@1337: sync.enable = 1; fp@1337: ec_sync_page(&sync, 0, slave->sii.boot_rx_mailbox_size, fp@1337: EC_DIR_INVALID, // use default direction fp@2123: 0, // no PDO xfer fp@1337: datagram->data); fp@1338: slave->configured_rx_mailbox_offset = fp@1338: slave->sii.boot_rx_mailbox_offset; fp@1338: slave->configured_rx_mailbox_size = fp@1338: slave->sii.boot_rx_mailbox_size; fp@1337: fp@1337: ec_sync_init(&sync, slave); fp@1337: sync.physical_start_address = slave->sii.boot_tx_mailbox_offset; fp@1337: sync.control_register = 0x22; fp@1337: sync.enable = 1; fp@1337: ec_sync_page(&sync, 1, slave->sii.boot_tx_mailbox_size, fp@1337: EC_DIR_INVALID, // use default direction fp@2123: 0, // no PDO xfer fp@1337: datagram->data + EC_SYNC_PAGE_SIZE); fp@1338: slave->configured_tx_mailbox_offset = fp@1338: slave->sii.boot_tx_mailbox_offset; fp@1338: slave->configured_tx_mailbox_size = fp@1338: slave->sii.boot_tx_mailbox_size; fp@1338: fp@1337: } else if (slave->sii.sync_count >= 2) { // mailbox configuration provided fp@830: ec_datagram_fpwr(datagram, slave->station_address, 0x0800, fp@834: EC_SYNC_PAGE_SIZE * slave->sii.sync_count); fp@1225: ec_datagram_zero(datagram); fp@830: fp@830: for (i = 0; i < 2; i++) { fp@1055: ec_sync_page(&slave->sii.syncs[i], i, fp@1055: slave->sii.syncs[i].default_length, fp@1509: NULL, // use default sync manager configuration fp@2123: 0, // no PDO xfer fp@830: datagram->data + EC_SYNC_PAGE_SIZE * i); fp@830: } fp@1338: fp@1338: slave->configured_rx_mailbox_offset = fp@1338: slave->sii.syncs[0].physical_start_address; fp@1338: slave->configured_rx_mailbox_size = fp@1338: slave->sii.syncs[0].default_length; fp@1338: slave->configured_tx_mailbox_offset = fp@1338: slave->sii.syncs[1].physical_start_address; fp@1338: slave->configured_tx_mailbox_size = fp@1338: slave->sii.syncs[1].default_length; fp@830: } else { // no mailbox sync manager configurations provided fp@830: ec_sync_t sync; fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Slave does not provide" fp@1921: " mailbox sync manager configurations.\n"); fp@830: fp@830: ec_datagram_fpwr(datagram, slave->station_address, 0x0800, fp@830: EC_SYNC_PAGE_SIZE * 2); fp@1225: ec_datagram_zero(datagram); fp@830: fp@1055: ec_sync_init(&sync, slave); fp@1337: sync.physical_start_address = slave->sii.std_rx_mailbox_offset; fp@830: sync.control_register = 0x26; fp@830: sync.enable = 1; fp@1337: ec_sync_page(&sync, 0, slave->sii.std_rx_mailbox_size, fp@1509: NULL, // use default sync manager configuration fp@2123: 0, // no PDO xfer fp@1055: datagram->data); fp@1338: slave->configured_rx_mailbox_offset = fp@1338: slave->sii.std_rx_mailbox_offset; fp@1338: slave->configured_rx_mailbox_size = fp@1338: slave->sii.std_rx_mailbox_size; fp@1055: fp@1055: ec_sync_init(&sync, slave); fp@1337: sync.physical_start_address = slave->sii.std_tx_mailbox_offset; fp@830: sync.control_register = 0x22; fp@830: sync.enable = 1; fp@1337: ec_sync_page(&sync, 1, slave->sii.std_tx_mailbox_size, fp@1509: NULL, // use default sync manager configuration fp@2123: 0, // no PDO xfer fp@1055: datagram->data + EC_SYNC_PAGE_SIZE); fp@1338: slave->configured_tx_mailbox_offset = fp@1471: slave->sii.std_tx_mailbox_offset; fp@1338: slave->configured_tx_mailbox_size = fp@1471: slave->sii.std_tx_mailbox_size; fp@830: } fp@830: fp@1347: fsm->take_time = 1; fp@1347: fp@830: fsm->retries = EC_FSM_RETRIES; fp@830: fsm->state = ec_fsm_slave_config_state_mbox_sync; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: SYNC. fp@1345: * fp@1345: * \todo Timeout for response. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_mbox_sync( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_datagram_t *datagram = fsm->datagram; fp@830: ec_slave_t *slave = fsm->slave; fp@830: fp@830: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@830: return; fp@830: fp@830: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive sync manager" fp@1921: " configuration datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@830: return; fp@830: } fp@830: fp@1347: if (fsm->take_time) { fp@1347: fsm->take_time = 0; fp@1347: fsm->jiffies_start = datagram->jiffies_sent; fp@1347: } fp@1347: fp@1347: /* Because the sync manager configurations are cleared during the last fp@1347: * cycle, some slaves do not immediately respond to the mailbox sync fp@1347: * manager configuration datagram. Therefore, resend the datagram for fp@1347: * a certain time, if the slave does not respond. fp@1347: */ fp@1347: if (datagram->working_counter == 0) { fp@1347: unsigned long diff = datagram->jiffies_received - fsm->jiffies_start; fp@1347: fp@1347: if (diff >= HZ) { fp@1347: slave->error_flag = 1; fp@1347: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Timeout while configuring" fp@1921: " mailbox sync managers.\n"); fp@1347: return; fp@1921: } else { fp@1921: EC_SLAVE_DBG(slave, 1, "Resending after %u ms...\n", fp@1347: (unsigned int) diff * 1000 / HZ); fp@1347: } fp@1347: fp@1347: // send configuration datagram again fp@1347: fsm->retries = EC_FSM_RETRIES; fp@1347: return; fp@1347: } fp@1347: else if (datagram->working_counter != 1) { fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to set sync managers: "); fp@830: ec_datagram_print_wc_error(datagram); fp@830: return; fp@830: } fp@830: fp@1337: ec_fsm_slave_config_enter_boot_preop(fsm); fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Request PREOP state. fp@1064: */ fp@1337: void ec_fsm_slave_config_enter_boot_preop( fp@1337: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1337: ) fp@1337: { fp@1337: fsm->state = ec_fsm_slave_config_state_boot_preop; fp@1337: fp@1337: if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) { fp@1337: ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); fp@1337: } else { // BOOT fp@1337: ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_BOOT); fp@1337: } fp@1337: fp@1174: ec_fsm_change_exec(fsm->fsm_change); // execute immediately fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1337: /** Slave configuration state: BOOT/PREOP. fp@1337: */ fp@1337: void ec_fsm_slave_config_state_boot_preop( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_slave_t *slave = fsm->slave; fp@830: fp@1174: if (ec_fsm_change_exec(fsm->fsm_change)) return; fp@1174: fp@1174: if (!ec_fsm_change_success(fsm->fsm_change)) { fp@1174: if (!fsm->fsm_change->spontaneous_change) fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@830: return; fp@830: } fp@830: fp@1404: // slave is now in BOOT or PREOP fp@830: slave->jiffies_preop = fsm->datagram->jiffies_received; fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Now in %s.\n", fp@1921: slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT"); fp@830: fp@830: if (slave->current_state == slave->requested_state) { fp@830: fsm->state = ec_fsm_slave_config_state_end; // successful fp@1921: EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); fp@830: return; fp@830: } fp@830: fp@1491: ec_fsm_slave_config_enter_sdo_conf(fsm); fp@1491: } fp@1491: fp@1491: /*****************************************************************************/ fp@1491: fp@1491: /** Check for SDO configurations to be applied. fp@1491: */ fp@1491: void ec_fsm_slave_config_enter_sdo_conf( fp@1491: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1491: ) fp@1491: { fp@1491: ec_slave_t *slave = fsm->slave; fp@1491: fp@830: if (!slave->config) { fp@1491: ec_fsm_slave_config_enter_pdo_sync(fsm); fp@1491: return; fp@1491: } fp@888: fp@830: // No CoE configuration to be applied? fp@1327: if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration fp@1944: ec_fsm_slave_config_enter_soe_conf_preop(fsm); fp@830: return; fp@830: } fp@830: fp@1327: // start SDO configuration fp@830: fsm->state = ec_fsm_slave_config_state_sdo_conf; fp@854: fsm->request = list_entry(fsm->slave->config->sdo_configs.next, fp@854: ec_sdo_request_t, list); fp@1318: ec_sdo_request_copy(&fsm->request_copy, fsm->request); fp@1318: ecrt_sdo_request_write(&fsm->request_copy); fp@1318: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: SDO_CONF. fp@1064: */ fp@830: void ec_fsm_slave_config_state_sdo_conf( fp@830: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@830: ) fp@830: { fp@1174: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1174: fp@1174: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1921: EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); fp@830: fsm->slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@830: return; fp@830: } fp@830: fp@1318: if (!fsm->slave->config) { // config removed in the meantime fp@1318: ec_fsm_slave_config_reconfigure(fsm); fp@1318: return; fp@1318: } fp@1318: fp@1327: // Another SDO to configure? fp@854: if (fsm->request->list.next != &fsm->slave->config->sdo_configs) { fp@1318: fsm->request = list_entry(fsm->request->list.next, fp@1318: ec_sdo_request_t, list); fp@1318: ec_sdo_request_copy(&fsm->request_copy, fsm->request); fp@1318: ecrt_sdo_request_write(&fsm->request_copy); fp@1318: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@830: return; fp@830: } fp@830: fp@1327: // All SDOs are now configured. fp@1944: ec_fsm_slave_config_enter_soe_conf_preop(fsm); fp@1844: } fp@1844: fp@1844: /*****************************************************************************/ fp@1844: fp@1844: /** Check for SoE configurations to be applied. fp@1844: */ fp@1944: void ec_fsm_slave_config_enter_soe_conf_preop( fp@1844: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1844: ) fp@1844: { fp@1844: ec_slave_t *slave = fsm->slave; fp@1873: ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; fp@1944: ec_soe_request_t *req; fp@1844: fp@1844: if (!slave->config) { fp@1844: ec_fsm_slave_config_enter_pdo_sync(fsm); fp@1844: return; fp@1844: } fp@1844: fp@1944: list_for_each_entry(req, &slave->config->soe_configs, list) { fp@1944: if (req->al_state == EC_AL_STATE_PREOP) { fp@1944: // start SoE configuration fp@1944: fsm->state = ec_fsm_slave_config_state_soe_conf_preop; fp@1944: fsm->soe_request = req; fp@1944: ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); fp@1944: ec_soe_request_write(&fsm->soe_request_copy); fp@1944: ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); fp@1944: ec_fsm_soe_exec(fsm_soe); // execute immediately fp@1944: ec_master_queue_external_datagram(slave->master, fp@2374: fsm_soe->datagram); fp@1944: return; fp@1944: } fp@1944: } fp@1944: fp@1944: // No SoE configuration to be applied in PREOP fp@1944: ec_fsm_slave_config_enter_pdo_conf(fsm); fp@1844: } fp@1844: fp@1844: /*****************************************************************************/ fp@1844: fp@1844: /** Slave configuration state: SOE_CONF. fp@1844: */ fp@1944: void ec_fsm_slave_config_state_soe_conf_preop( fp@1844: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1844: ) fp@1844: { fp@1844: ec_slave_t *slave = fsm->slave; fp@1873: ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; fp@1844: fp@1844: if (ec_fsm_soe_exec(fsm_soe)) { fp@2374: ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); fp@1873: return; fp@1873: } fp@1844: fp@1844: if (!ec_fsm_soe_success(fsm_soe)) { fp@1921: EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); fp@1844: fsm->slave->error_flag = 1; fp@1844: fsm->state = ec_fsm_slave_config_state_error; fp@1844: return; fp@1844: } fp@1844: fp@1844: if (!fsm->slave->config) { // config removed in the meantime fp@1844: ec_fsm_slave_config_reconfigure(fsm); fp@1844: return; fp@1844: } fp@1844: fp@1944: // Another IDN to configure in PREOP? fp@1944: while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { fp@1844: fsm->soe_request = list_entry(fsm->soe_request->list.next, fp@1844: ec_soe_request_t, list); fp@1944: if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { fp@1944: ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); fp@1944: ec_soe_request_write(&fsm->soe_request_copy); fp@1944: ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); fp@1944: ec_fsm_soe_exec(fsm_soe); // execute immediately fp@1944: ec_master_queue_external_datagram(slave->master, fp@2374: fsm_soe->datagram); fp@1944: return; fp@1944: } fp@1944: } fp@1944: fp@1944: // All PREOP IDNs are now configured. fp@1180: ec_fsm_slave_config_enter_pdo_conf(fsm); fp@1180: } fp@1180: fp@1180: /*****************************************************************************/ fp@1180: fp@1180: /** PDO_CONF entry function. fp@1180: */ fp@1180: void ec_fsm_slave_config_enter_pdo_conf( fp@1180: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1180: ) fp@1180: { fp@1327: // Start configuring PDOs fp@1180: ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave); fp@1180: fsm->state = ec_fsm_slave_config_state_pdo_conf; fp@1180: fsm->state(fsm); // execute immediately fp@1180: } fp@1180: fp@1180: /*****************************************************************************/ fp@1180: fp@1180: /** Slave configuration state: PDO_CONF. fp@1180: */ fp@1180: void ec_fsm_slave_config_state_pdo_conf( fp@1180: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1180: ) fp@1180: { fp@1507: // TODO check for config here fp@1407: fp@1180: if (ec_fsm_pdo_exec(fsm->fsm_pdo)) fp@1180: return; fp@1180: fp@1318: if (!fsm->slave->config) { // config removed in the meantime fp@1318: ec_fsm_slave_config_reconfigure(fsm); fp@1318: return; fp@1318: } fp@1318: fp@1180: if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { fp@1921: EC_SLAVE_WARN(fsm->slave, "PDO configuration failed.\n"); fp@1180: } fp@1180: fp@1509: ec_fsm_slave_config_enter_watchdog_divider(fsm); fp@1509: } fp@1509: fp@1509: /*****************************************************************************/ fp@1509: fp@1509: /** WATCHDOG_DIVIDER entry function. fp@1509: */ fp@1509: void ec_fsm_slave_config_enter_watchdog_divider( fp@1509: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1509: ) fp@1509: { fp@1509: ec_slave_t *slave = fsm->slave; fp@1509: ec_datagram_t *datagram = fsm->datagram; fp@1509: ec_slave_config_t *config = slave->config; fp@1509: fp@1509: if (config && config->watchdog_divider) { fp@1921: EC_SLAVE_DBG(slave, 1, "Setting watchdog divider to %u.\n", fp@1921: config->watchdog_divider); fp@1509: fp@1509: ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2); fp@1509: EC_WRITE_U16(datagram->data, config->watchdog_divider); fp@1509: fsm->retries = EC_FSM_RETRIES; fp@1509: fsm->state = ec_fsm_slave_config_state_watchdog_divider; fp@1509: } else { fp@1509: ec_fsm_slave_config_enter_watchdog(fsm); fp@1509: } fp@1509: } fp@1509: fp@1509: /*****************************************************************************/ fp@1509: fp@1509: /** Slave configuration state: WATCHDOG_DIVIDER. fp@1509: */ fp@1509: void ec_fsm_slave_config_state_watchdog_divider( fp@1509: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1509: ) fp@1509: { fp@1509: ec_datagram_t *datagram = fsm->datagram; fp@1509: ec_slave_t *slave = fsm->slave; fp@1509: fp@1509: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1509: return; fp@1509: fp@1509: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1509: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive watchdog divider" fp@1921: " configuration datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@1509: return; fp@1509: } fp@1509: fp@1509: if (datagram->working_counter != 1) { fp@1509: slave->error_flag = 1; fp@1921: EC_SLAVE_WARN(slave, "Failed to set watchdog divider: "); fp@1509: ec_datagram_print_wc_error(datagram); fp@1509: return; fp@1509: } fp@1509: fp@1509: ec_fsm_slave_config_enter_watchdog(fsm); fp@1509: } fp@1509: fp@1509: /*****************************************************************************/ fp@1509: fp@1509: /** WATCHDOG entry function fp@1509: */ fp@1509: void ec_fsm_slave_config_enter_watchdog( fp@1509: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1509: ) fp@1509: { fp@1509: ec_datagram_t *datagram = fsm->datagram; fp@1509: ec_slave_t *slave = fsm->slave; fp@1509: ec_slave_config_t *config = slave->config; fp@1509: fp@1509: if (config && config->watchdog_intervals) { fp@1921: EC_SLAVE_DBG(slave, 1, "Setting process data" fp@1921: " watchdog intervals to %u.\n", config->watchdog_intervals); fp@1509: fp@1509: ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2); fp@1509: EC_WRITE_U16(datagram->data, config->watchdog_intervals); fp@1509: fp@1509: fsm->retries = EC_FSM_RETRIES; fp@1509: fsm->state = ec_fsm_slave_config_state_watchdog; fp@1520: } else { fp@1520: ec_fsm_slave_config_enter_pdo_sync(fsm); fp@1520: } fp@1509: } fp@1509: fp@1509: /*****************************************************************************/ fp@1509: fp@1509: /** Slave configuration state: WATCHDOG. fp@1509: */ fp@1509: fp@1509: void ec_fsm_slave_config_state_watchdog( fp@1509: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1509: ) fp@1509: { fp@1509: ec_datagram_t *datagram = fsm->datagram; fp@1509: ec_slave_t *slave = fsm->slave; fp@1509: fp@1509: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1509: return; fp@1509: fp@1509: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1509: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive sync manager" fp@1921: " watchdog configuration datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@1509: return; fp@1509: } fp@1509: fp@1509: if (datagram->working_counter != 1) { fp@1921: EC_SLAVE_WARN(slave, "Failed to set process data" fp@1921: " watchdog intervals: "); fp@1509: ec_datagram_print_wc_error(datagram); fp@1509: } fp@1509: fp@888: ec_fsm_slave_config_enter_pdo_sync(fsm); fp@888: } fp@888: fp@888: /*****************************************************************************/ fp@888: fp@1327: /** Check for PDO sync managers to be configured. fp@1064: */ fp@888: void ec_fsm_slave_config_enter_pdo_sync( fp@888: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@888: ) fp@888: { fp@888: ec_slave_t *slave = fsm->slave; fp@888: ec_datagram_t *datagram = fsm->datagram; fp@2123: unsigned int i, j, offset, num_pdo_syncs; fp@1055: uint8_t sync_index; fp@888: const ec_sync_t *sync; fp@888: uint16_t size; fp@888: fp@888: if (slave->sii.mailbox_protocols) { fp@888: offset = 2; // slave has mailboxes fp@888: } else { fp@888: offset = 0; fp@888: } fp@888: fp@888: if (slave->sii.sync_count <= offset) { fp@1327: // no PDO sync managers to configure fp@1180: ec_fsm_slave_config_enter_fmmu(fsm); fp@888: return; fp@888: } fp@888: fp@888: num_pdo_syncs = slave->sii.sync_count - offset; fp@888: fp@888: // configure sync managers for process data fp@888: ec_datagram_fpwr(datagram, slave->station_address, fp@888: 0x0800 + EC_SYNC_PAGE_SIZE * offset, fp@888: EC_SYNC_PAGE_SIZE * num_pdo_syncs); fp@1225: ec_datagram_zero(datagram); fp@888: fp@888: for (i = 0; i < num_pdo_syncs; i++) { fp@1509: const ec_sync_config_t *sync_config; fp@2123: uint8_t pdo_xfer = 0; fp@1055: sync_index = i + offset; fp@1055: sync = &slave->sii.syncs[sync_index]; fp@1491: fp@1491: if (slave->config) { fp@2123: const ec_slave_config_t *sc = slave->config; fp@2123: sync_config = &sc->sync_configs[sync_index]; fp@1491: size = ec_pdo_list_total_size(&sync_config->pdos); fp@2123: fp@2123: // determine, if PDOs shall be transferred via this SM fp@2123: // inthat case, enable sync manager in every case fp@2123: for (j = 0; j < sc->used_fmmus; j++) { fp@2123: if (sc->fmmu_configs[j].sync_index == sync_index) { fp@2123: pdo_xfer = 1; fp@2123: break; fp@2123: } fp@2123: } fp@2123: fp@1491: } else { fp@1509: sync_config = NULL; fp@1491: size = sync->default_length; fp@1491: } fp@1491: fp@2123: ec_sync_page(sync, sync_index, size, sync_config, pdo_xfer, fp@1509: datagram->data + EC_SYNC_PAGE_SIZE * i); fp@888: } fp@888: fp@888: fsm->retries = EC_FSM_RETRIES; fp@888: fsm->state = ec_fsm_slave_config_state_pdo_sync; fp@888: } fp@888: fp@888: /*****************************************************************************/ fp@888: fp@1327: /** Configure PDO sync managers. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_pdo_sync( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@888: { fp@888: ec_datagram_t *datagram = fsm->datagram; fp@888: ec_slave_t *slave = fsm->slave; fp@888: fp@888: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@888: return; fp@888: fp@888: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@888: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive process data sync" fp@1921: " manager configuration datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@888: return; fp@888: } fp@888: fp@888: if (datagram->working_counter != 1) { fp@888: slave->error_flag = 1; fp@888: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to set process data sync managers: "); fp@888: ec_datagram_print_wc_error(datagram); fp@888: return; fp@888: } fp@888: fp@830: ec_fsm_slave_config_enter_fmmu(fsm); fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Check for FMMUs to be configured. fp@1064: */ fp@1064: void ec_fsm_slave_config_enter_fmmu( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_slave_t *slave = fsm->slave; fp@830: ec_datagram_t *datagram = fsm->datagram; fp@830: unsigned int i; fp@830: const ec_fmmu_config_t *fmmu; fp@830: const ec_sync_t *sync; fp@830: fp@1491: if (!slave->config) { fp@1491: ec_fsm_slave_config_enter_safeop(fsm); fp@1407: return; fp@1407: } fp@1407: fp@830: if (slave->base_fmmu_count < slave->config->used_fmmus) { fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Slave has less FMMUs (%u)" fp@1921: " than requested (%u).\n", slave->base_fmmu_count, fp@830: slave->config->used_fmmus); fp@830: return; fp@830: } fp@830: fp@830: if (!slave->base_fmmu_count) { // skip FMMU configuration fp@1407: ec_fsm_slave_config_enter_dc_cycle(fsm); fp@830: return; fp@830: } fp@830: fp@830: // configure FMMUs fp@830: ec_datagram_fpwr(datagram, slave->station_address, fp@830: 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); fp@1225: ec_datagram_zero(datagram); fp@830: for (i = 0; i < slave->config->used_fmmus; i++) { fp@830: fmmu = &slave->config->fmmu_configs[i]; fp@1055: if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) { fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to determine PDO sync manager" fp@1921: " for FMMU!\n"); fp@830: return; fp@830: } fp@830: ec_fmmu_config_page(fmmu, sync, fp@830: datagram->data + EC_FMMU_PAGE_SIZE * i); fp@830: } fp@830: fp@830: fsm->retries = EC_FSM_RETRIES; fp@830: fsm->state = ec_fsm_slave_config_state_fmmu; fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: FMMU. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_fmmu( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_datagram_t *datagram = fsm->datagram; fp@830: ec_slave_t *slave = fsm->slave; fp@830: fp@830: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@830: return; fp@830: fp@830: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive FMMUs datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@830: return; fp@830: } fp@830: fp@830: if (datagram->working_counter != 1) { fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to set FMMUs: "); fp@830: ec_datagram_print_wc_error(datagram); fp@830: return; fp@830: } fp@830: fp@1407: ec_fsm_slave_config_enter_dc_cycle(fsm); fp@1407: } fp@1407: fp@1407: /*****************************************************************************/ fp@1407: fp@1407: /** Check for DC to be configured. fp@1407: */ fp@1407: void ec_fsm_slave_config_enter_dc_cycle( fp@1407: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1407: ) fp@1407: { fp@1407: ec_datagram_t *datagram = fsm->datagram; fp@1407: ec_slave_t *slave = fsm->slave; fp@1407: ec_slave_config_t *config = slave->config; fp@1407: fp@1407: if (!config) { // config removed in the meantime fp@1407: ec_fsm_slave_config_reconfigure(fsm); fp@1396: return; fp@1396: } fp@1392: fp@1392: if (config->dc_assign_activate) { fp@1419: if (!slave->base_dc_supported || !slave->has_dc_system_time) { fp@1921: EC_SLAVE_WARN(slave, "Slave seems not to support" fp@1921: " distributed clocks!\n"); fp@1407: } fp@1407: fp@1921: EC_SLAVE_DBG(slave, 1, "Setting DC cycle times to %u / %u.\n", fp@1921: config->dc_sync[0].cycle_time, config->dc_sync[1].cycle_time); fp@1458: fp@1392: // set DC cycle times fp@1392: ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); fp@1438: EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time); fp@1438: EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time); fp@1392: fsm->retries = EC_FSM_RETRIES; fp@1392: fsm->state = ec_fsm_slave_config_state_dc_cycle; fp@1392: } else { fp@1404: // DC are unused fp@1396: ec_fsm_slave_config_enter_safeop(fsm); fp@1392: } fp@1392: } fp@1392: fp@1392: /*****************************************************************************/ fp@1392: fp@1392: /** Slave configuration state: DC CYCLE. fp@1392: */ fp@1392: void ec_fsm_slave_config_state_dc_cycle( fp@1392: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1392: ) fp@1392: { fp@1392: ec_datagram_t *datagram = fsm->datagram; fp@1392: ec_slave_t *slave = fsm->slave; fp@1925: ec_slave_config_t *config = slave->config; fp@1925: fp@1925: if (!config) { // config removed in the meantime fp@1925: ec_fsm_slave_config_reconfigure(fsm); fp@1925: return; fp@1925: } fp@1925: fp@1925: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1925: return; fp@1925: fp@1925: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1925: fsm->state = ec_fsm_slave_config_state_error; fp@1925: EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: "); fp@1925: ec_datagram_print_state(datagram); fp@1925: return; fp@1925: } fp@1925: fp@1925: if (datagram->working_counter != 1) { fp@1925: slave->error_flag = 1; fp@1925: fsm->state = ec_fsm_slave_config_state_error; fp@1925: EC_SLAVE_ERR(slave, "Failed to set DC cycle times: "); fp@1925: ec_datagram_print_wc_error(datagram); fp@1925: return; fp@1925: } fp@1925: fp@1925: EC_SLAVE_DBG(slave, 1, "Checking for synchrony.\n"); fp@1925: fp@1925: fsm->jiffies_start = jiffies; fp@1925: ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); fp@1925: fsm->retries = EC_FSM_RETRIES; fp@1925: fsm->state = ec_fsm_slave_config_state_dc_sync_check; fp@1925: } fp@1925: fp@1925: /*****************************************************************************/ fp@1925: fp@1925: /** Slave configuration state: DC SYNC CHECK. fp@1925: */ fp@1925: void ec_fsm_slave_config_state_dc_sync_check( fp@1925: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1925: ) fp@1925: { fp@1925: ec_datagram_t *datagram = fsm->datagram; fp@1925: ec_slave_t *slave = fsm->slave; fp@1436: ec_master_t *master = slave->master; fp@1412: ec_slave_config_t *config = slave->config; fp@1925: uint32_t abs_sync_diff; fp@1925: unsigned long diff_ms; fp@1438: ec_sync_signal_t *sync0 = &config->dc_sync[0]; fp@1396: u64 start_time; fp@1392: fp@1412: if (!config) { // config removed in the meantime fp@1412: ec_fsm_slave_config_reconfigure(fsm); fp@1412: return; fp@1412: } fp@1412: fp@1392: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1392: return; fp@1392: fp@1392: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1392: fsm->state = ec_fsm_slave_config_state_error; fp@1925: EC_SLAVE_ERR(slave, "Failed to receive DC sync check datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@1392: return; fp@1392: } fp@1392: fp@1392: if (datagram->working_counter != 1) { fp@1392: slave->error_flag = 1; fp@1392: fsm->state = ec_fsm_slave_config_state_error; fp@1925: EC_SLAVE_ERR(slave, "Failed to check DC synchrony: "); fp@1392: ec_datagram_print_wc_error(datagram); fp@1392: return; fp@1392: } fp@1392: fp@1925: abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; fp@1925: diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; fp@1925: fp@1925: if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { fp@1925: fp@1925: if (diff_ms >= EC_DC_SYNC_WAIT_MS) { fp@1931: EC_SLAVE_WARN(slave, "Slave did not sync after %lu ms.\n", fp@1931: diff_ms); fp@1925: } else { fp@1931: EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n", fp@1931: diff_ms, abs_sync_diff); fp@1925: fp@1925: // check synchrony again fp@1925: ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); fp@1925: fsm->retries = EC_FSM_RETRIES; fp@1925: return; fp@1925: } fp@1925: } else { fp@1931: EC_SLAVE_DBG(slave, 1, "%u ns difference after %lu ms.\n", fp@1931: abs_sync_diff, diff_ms); fp@1925: } fp@1925: fp@1392: // set DC start time fp@1925: start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns fp@1436: // FIXME use slave's local system time here? fp@1436: fp@1438: if (sync0->cycle_time) { fp@1436: // find correct phase fp@1925: if (master->has_app_time) { fp@1438: u64 diff, start; fp@1438: u32 remainder; fp@1438: fp@1436: diff = start_time - master->app_start_time; fp@1438: remainder = do_div(diff, sync0->cycle_time); fp@1438: fp@1438: start = start_time + fp@1438: sync0->cycle_time - remainder + sync0->shift_time; fp@1436: fp@1925: EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n", fp@1925: master->app_start_time); fp@2104: EC_SLAVE_DBG(slave, 1, " app_time=%llu\n", master->app_time); fp@1925: EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", start_time); fp@1925: EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", sync0->cycle_time); fp@1925: EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", sync0->shift_time); fp@1925: EC_SLAVE_DBG(slave, 1, " remainder=%u\n", remainder); fp@1925: EC_SLAVE_DBG(slave, 1, " start=%llu\n", start); fp@1436: start_time = start; fp@1436: } else { fp@1921: EC_SLAVE_WARN(slave, "No application time supplied." fp@1921: " Cyclic start time will not be in phase.\n"); fp@1436: } fp@1436: } fp@1436: fp@1921: EC_SLAVE_DBG(slave, 1, "Setting DC cyclic operation" fp@1921: " start time to %llu.\n", start_time); fp@1396: fp@1392: ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); fp@1396: EC_WRITE_U64(datagram->data, start_time); fp@1392: fsm->retries = EC_FSM_RETRIES; fp@1392: fsm->state = ec_fsm_slave_config_state_dc_start; fp@1392: } fp@1392: fp@1392: /*****************************************************************************/ fp@1392: fp@1392: /** Slave configuration state: DC START. fp@1392: */ fp@1392: void ec_fsm_slave_config_state_dc_start( fp@1392: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1392: ) fp@1392: { fp@1392: ec_datagram_t *datagram = fsm->datagram; fp@1392: ec_slave_t *slave = fsm->slave; fp@1407: ec_slave_config_t *config = slave->config; fp@1407: fp@1407: if (!config) { // config removed in the meantime fp@1407: ec_fsm_slave_config_reconfigure(fsm); fp@1407: return; fp@1407: } fp@1392: fp@1392: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1392: return; fp@1392: fp@1392: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1392: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive DC start time datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@1392: return; fp@1392: } fp@1392: fp@1392: if (datagram->working_counter != 1) { fp@1392: slave->error_flag = 1; fp@1392: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to set DC start time: "); fp@1392: ec_datagram_print_wc_error(datagram); fp@1392: return; fp@1392: } fp@1392: fp@1921: EC_SLAVE_DBG(slave, 1, "Setting DC AssignActivate to 0x%04x.\n", fp@1921: config->dc_assign_activate); fp@1458: fp@1392: // assign sync unit to EtherCAT or PDI fp@1392: ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); fp@1392: EC_WRITE_U16(datagram->data, config->dc_assign_activate); fp@1392: fsm->retries = EC_FSM_RETRIES; fp@1392: fsm->state = ec_fsm_slave_config_state_dc_assign; fp@1392: } fp@1392: fp@1392: /*****************************************************************************/ fp@1392: fp@1392: /** Slave configuration state: DC ASSIGN. fp@1392: */ fp@1392: void ec_fsm_slave_config_state_dc_assign( fp@1392: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1392: ) fp@1392: { fp@1392: ec_datagram_t *datagram = fsm->datagram; fp@1392: ec_slave_t *slave = fsm->slave; fp@1392: fp@1392: if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) fp@1392: return; fp@1392: fp@1392: if (datagram->state != EC_DATAGRAM_RECEIVED) { fp@1392: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to receive DC activation datagram: "); fp@1822: ec_datagram_print_state(datagram); fp@1392: return; fp@1392: } fp@1392: fp@1392: if (datagram->working_counter != 1) { fp@1392: slave->error_flag = 1; fp@1392: fsm->state = ec_fsm_slave_config_state_error; fp@1921: EC_SLAVE_ERR(slave, "Failed to activate DC: "); fp@1392: ec_datagram_print_wc_error(datagram); fp@1392: return; fp@1392: } fp@1392: fp@830: ec_fsm_slave_config_enter_safeop(fsm); fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Request SAFEOP state. fp@1064: */ fp@1064: void ec_fsm_slave_config_enter_safeop( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: fsm->state = ec_fsm_slave_config_state_safeop; fp@1174: ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP); fp@1174: ec_fsm_change_exec(fsm->fsm_change); // execute immediately fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: SAFEOP. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_safeop( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_slave_t *slave = fsm->slave; fp@830: fp@1174: if (ec_fsm_change_exec(fsm->fsm_change)) return; fp@1174: fp@1174: if (!ec_fsm_change_success(fsm->fsm_change)) { fp@1174: if (!fsm->fsm_change->spontaneous_change) fp@830: fsm->slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@830: return; fp@830: } fp@830: fp@830: // slave is now in SAFEOP fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Now in SAFEOP.\n"); fp@830: fp@830: if (fsm->slave->current_state == fsm->slave->requested_state) { fp@830: fsm->state = ec_fsm_slave_config_state_end; // successful fp@1921: EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); fp@830: return; fp@830: } fp@830: fp@1944: ec_fsm_slave_config_enter_soe_conf_safeop(fsm); fp@1944: } fp@1944: fp@1944: /*****************************************************************************/ fp@1944: fp@1944: /** Check for SoE configurations to be applied in SAFEOP. fp@1944: */ fp@1944: void ec_fsm_slave_config_enter_soe_conf_safeop( fp@1944: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1944: ) fp@1944: { fp@1944: ec_slave_t *slave = fsm->slave; fp@1944: ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; fp@1944: ec_soe_request_t *req; fp@1944: fp@1944: if (!slave->config) { fp@1944: ec_fsm_slave_config_enter_op(fsm); fp@1944: return; fp@1944: } fp@1944: fp@1944: list_for_each_entry(req, &slave->config->soe_configs, list) { fp@1944: if (req->al_state == EC_AL_STATE_SAFEOP) { fp@1944: // start SoE configuration fp@1944: fsm->state = ec_fsm_slave_config_state_soe_conf_safeop; fp@1944: fsm->soe_request = req; fp@1944: ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); fp@1944: ec_soe_request_write(&fsm->soe_request_copy); fp@1944: ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); fp@1944: ec_fsm_soe_exec(fsm_soe); // execute immediately fp@1944: ec_master_queue_external_datagram(slave->master, fp@2374: fsm_soe->datagram); fp@1944: return; fp@1944: } fp@1944: } fp@1944: fp@1944: // No SoE configuration to be applied in SAFEOP fp@1944: ec_fsm_slave_config_enter_op(fsm); fp@1944: } fp@1944: fp@1944: /*****************************************************************************/ fp@1944: fp@1944: /** Slave configuration state: SOE_CONF. fp@1944: */ fp@1944: void ec_fsm_slave_config_state_soe_conf_safeop( fp@1944: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1944: ) fp@1944: { fp@1944: ec_slave_t *slave = fsm->slave; fp@1944: ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; fp@1944: fp@1944: if (ec_fsm_soe_exec(fsm_soe)) { fp@2374: ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); fp@1944: return; fp@1944: } fp@1944: fp@1944: if (!ec_fsm_soe_success(fsm_soe)) { fp@1944: EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); fp@1944: fsm->slave->error_flag = 1; fp@1944: fsm->state = ec_fsm_slave_config_state_error; fp@1944: return; fp@1944: } fp@1944: fp@1944: if (!fsm->slave->config) { // config removed in the meantime fp@1944: ec_fsm_slave_config_reconfigure(fsm); fp@1944: return; fp@1944: } fp@1944: fp@1944: // Another IDN to configure in SAFEOP? fp@1944: while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) { fp@1944: fsm->soe_request = list_entry(fsm->soe_request->list.next, fp@1944: ec_soe_request_t, list); fp@1944: if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { fp@1944: ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); fp@1944: ec_soe_request_write(&fsm->soe_request_copy); fp@1944: ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); fp@1944: ec_fsm_soe_exec(fsm_soe); // execute immediately fp@1944: ec_master_queue_external_datagram(slave->master, fp@2374: fsm_soe->datagram); fp@1944: return; fp@1944: } fp@1944: } fp@1944: fp@1944: // All SAFEOP IDNs are now configured. fp@1944: ec_fsm_slave_config_enter_op(fsm); fp@1944: } fp@1944: fp@1944: /*****************************************************************************/ fp@1944: fp@1944: /** Bring slave to OP. fp@1944: */ fp@1944: void ec_fsm_slave_config_enter_op( fp@1944: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1944: ) fp@1944: { fp@830: // set state to OP fp@830: fsm->state = ec_fsm_slave_config_state_op; fp@1944: ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_OP); fp@1174: ec_fsm_change_exec(fsm->fsm_change); // execute immediately fp@830: } fp@830: fp@830: /*****************************************************************************/ fp@830: fp@1064: /** Slave configuration state: OP fp@1064: */ fp@1064: void ec_fsm_slave_config_state_op( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@830: { fp@830: ec_slave_t *slave = fsm->slave; fp@830: fp@1174: if (ec_fsm_change_exec(fsm->fsm_change)) return; fp@1174: fp@1174: if (!ec_fsm_change_success(fsm->fsm_change)) { fp@1174: if (!fsm->fsm_change->spontaneous_change) fp@830: slave->error_flag = 1; fp@830: fsm->state = ec_fsm_slave_config_state_error; fp@830: return; fp@830: } fp@830: fp@830: // slave is now in OP fp@830: fp@1921: EC_SLAVE_DBG(slave, 1, "Now in OP. Finished configuration.\n"); fp@830: fp@830: fsm->state = ec_fsm_slave_config_state_end; // successful fp@830: } fp@830: fp@1318: /*****************************************************************************/ fp@1318: fp@1318: /** Reconfigure the slave starting at INIT. fp@1318: */ fp@1318: void ec_fsm_slave_config_reconfigure( fp@1318: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1318: ) fp@1318: { fp@1921: EC_SLAVE_DBG(fsm->slave, 1, "Slave configuration detached during " fp@1921: "configuration. Reconfiguring."); fp@1318: fp@1318: ec_fsm_slave_config_enter_init(fsm); // reconfigure fp@1318: } fp@1318: fp@830: /****************************************************************************** fp@830: * Common state functions fp@830: *****************************************************************************/ fp@830: fp@1064: /** State: ERROR. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_error( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@1064: { fp@1064: } fp@1064: fp@1064: /*****************************************************************************/ fp@1064: fp@1064: /** State: END. fp@1064: */ fp@1064: void ec_fsm_slave_config_state_end( fp@1064: ec_fsm_slave_config_t *fsm /**< slave state machine */ fp@1064: ) fp@1064: { fp@1064: } fp@1064: fp@1064: /*****************************************************************************/