From ea7aa51ed1ff22339c8c01e9d061c457228aac99 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Tue, 19 Feb 2008 14:06:50 +0000 Subject: [PATCH] Pdo configuration. --- NEWS | 1 + TODO | 1 - documentation/graphs/fsm_slave_conf.dot | 8 +- examples/mini/mini.c | 2 +- master/Kbuild.in | 2 +- master/Makefile.am | 1 + master/fsm_pdo_config.c | 369 ++++++++++++++++++++++++ master/fsm_pdo_config.h | 83 ++++++ master/fsm_slave.c | 56 ++-- master/fsm_slave.h | 28 +- master/pdo.c | 53 ++++ master/pdo.h | 2 + master/pdo_mapping.c | 20 ++ master/pdo_mapping.h | 2 + master/slave.c | 28 +- master/slave.h | 1 + 16 files changed, 616 insertions(+), 41 deletions(-) create mode 100644 master/fsm_pdo_config.c create mode 100644 master/fsm_pdo_config.h diff --git a/NEWS b/NEWS index d4a4f23f..b89943e2 100644 --- a/NEWS +++ b/NEWS @@ -40,6 +40,7 @@ Changes in version 1.4.0: - Removed ecrt_domain_register_pdo_range(), because it's functionality can be reached by specifying an explicit Pdo mapping and registering those Pdo entries. +* Pdo configuration is now supported. * Added 8139too driver for kernel 2.6.22, thanks to Erwin Burgstaller. * Added e1000 driver for kernel 2.6.22. * Allow gaps in PDO mapping read from CoE. diff --git a/TODO b/TODO index 52600360..d17e2a36 100644 --- a/TODO +++ b/TODO @@ -9,7 +9,6 @@ $Id$ Version 1.4.0: * Realtime interface changes: - - PDO configuration. - SDO access. - External memory for domains. * Mailbox handler diff --git a/documentation/graphs/fsm_slave_conf.dot b/documentation/graphs/fsm_slave_conf.dot index dfe080ce..6f8ea492 100644 --- a/documentation/graphs/fsm_slave_conf.dot +++ b/documentation/graphs/fsm_slave_conf.dot @@ -34,11 +34,13 @@ digraph slaveconf { sdo_conf -> sdo_conf sdo_conf -> error sdo_conf -> enter_mapping [weight=10] - enter_mapping -> mapping [label="CoE supported", weight=10] - enter_mapping -> enter_pdo_sync + enter_mapping -> mapping [weight=10] mapping -> mapping mapping -> error - mapping -> enter_pdo_sync [weight=10] + mapping -> pdo_conf [weight=10] + pdo_conf -> pdo_conf + pdo_conf -> error + pdo_conf -> enter_pdo_sync [weight=10] enter_pdo_sync -> pdo_sync [label="PDO SMs", weight=10] enter_pdo_sync -> enter_fmmu pdo_sync -> pdo_sync diff --git a/examples/mini/mini.c b/examples/mini/mini.c index df6ce92b..75416d30 100644 --- a/examples/mini/mini.c +++ b/examples/mini/mini.c @@ -82,7 +82,7 @@ const ec_pdo_info_t mapping[] = { #ifdef PDOS static uint8_t off_ana_in; -static uint8_t off_ana_out; +//static uint8_t off_ana_out; const static ec_pdo_entry_reg_t domain1_regs[] = { {0, 1, Beckhoff_EL3162, 0x3101, 2, &off_ana_in}, diff --git a/master/Kbuild.in b/master/Kbuild.in index 821ab2a7..d733b506 100644 --- a/master/Kbuild.in +++ b/master/Kbuild.in @@ -36,7 +36,7 @@ obj-m := ec_master.o ec_master-objs := module.o master.o device.o pdo.o sync.o fmmu_config.o \ slave.o slave_config.o pdo_mapping.o datagram.o domain.o mailbox.o \ canopen.o fsm_sii.o fsm_change.o fsm_coe.o fsm_coe_map.o fsm_mapping.o \ - fsm_slave.o fsm_master.o xmldev.o + fsm_pdo_config.o fsm_slave.o fsm_master.o xmldev.o ifeq (@ENABLE_EOE@,1) ec_master-objs += ethernet.o diff --git a/master/Makefile.am b/master/Makefile.am index 1c236e1c..b04e7902 100644 --- a/master/Makefile.am +++ b/master/Makefile.am @@ -48,6 +48,7 @@ EXTRA_DIST = \ fsm_coe.c fsm_coe.h \ fsm_coe_map.c fsm_coe_map.h \ fsm_mapping.c fsm_mapping.h \ + fsm_pdo_config.c fsm_pdo_config.h \ fsm_slave.c fsm_slave.h \ fsm_master.c fsm_master.h \ globals.h \ diff --git a/master/fsm_pdo_config.c b/master/fsm_pdo_config.c new file mode 100644 index 00000000..123ee768 --- /dev/null +++ b/master/fsm_pdo_config.c @@ -0,0 +1,369 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it + * and/or modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * The IgH EtherCAT Master is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * The right to use EtherCAT Technology is granted and comes free of + * charge under condition of compatibility of product made by + * Licensee. People intending to distribute/sell products based on the + * code, have to sign an agreement to guarantee that products using + * software based on IgH EtherCAT master stay compatible with the actual + * EtherCAT specification (which are released themselves as an open + * standard) as the (only) precondition to have the right to use EtherCAT + * Technology, IP and trade marks. + * + *****************************************************************************/ + +/** \file + * EtherCAT Pdo configuration state machine. + */ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "mailbox.h" +#include "slave_config.h" + +#include "fsm_pdo_config.h" + +/*****************************************************************************/ + +void ec_fsm_pdo_config_state_start(ec_fsm_pdo_config_t *); +void ec_fsm_pdo_config_state_zero_count(ec_fsm_pdo_config_t *); +void ec_fsm_pdo_config_state_add_entry(ec_fsm_pdo_config_t *); +void ec_fsm_pdo_config_state_entry_count(ec_fsm_pdo_config_t *); +void ec_fsm_pdo_config_state_end(ec_fsm_pdo_config_t *); +void ec_fsm_pdo_config_state_error(ec_fsm_pdo_config_t *); + +void ec_fsm_pdo_config_next_pdo(ec_fsm_pdo_config_t *); + +/*****************************************************************************/ + +/** Constructor. + */ +void ec_fsm_pdo_config_init( + ec_fsm_pdo_config_t *fsm, /**< pdo_config state machine */ + ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */ + ) +{ + fsm->fsm_coe = fsm_coe; + fsm->sdodata.data = (uint8_t *) &fsm->sdo_value; +} + +/*****************************************************************************/ + +/** Destructor. + */ +void ec_fsm_pdo_config_clear( + ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */ + ) +{ +} + +/*****************************************************************************/ + +/** Start Pdo configuration state machine. + */ +void ec_fsm_pdo_config_start( + ec_fsm_pdo_config_t *fsm, /**< Pdo configuration state machine */ + ec_slave_t *slave /**< slave to configure */ + ) +{ + fsm->slave = slave; + fsm->state = ec_fsm_pdo_config_state_start; +} + +/*****************************************************************************/ + +/** Get running state. + * + * \return false, if state machine has terminated + */ +int ec_fsm_pdo_config_running( + const ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + return fsm->state != ec_fsm_pdo_config_state_end + && fsm->state != ec_fsm_pdo_config_state_error; +} + +/*****************************************************************************/ + +/** Executes the current state. + * + * \return false, if state machine has terminated + */ +int ec_fsm_pdo_config_exec( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + fsm->state(fsm); + return ec_fsm_pdo_config_running(fsm); +} + +/*****************************************************************************/ + +/** Get execution result. + * + * \return true, if the state machine terminated gracefully + */ +int ec_fsm_pdo_config_success( + const ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + return fsm->state == ec_fsm_pdo_config_state_end; +} + +/****************************************************************************** + * State functions. + *****************************************************************************/ + +/** Start Pdo configuration. + */ +void ec_fsm_pdo_config_state_start( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + if (!fsm->slave->config) { + fsm->state = ec_fsm_pdo_config_state_end; + return; + } + + fsm->pdo = NULL; + ec_fsm_pdo_config_next_pdo(fsm); +} + +/*****************************************************************************/ + +/** Process configuration of next Pdo. + */ +void ec_fsm_pdo_config_next_pdo( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + ec_direction_t dir; + const ec_pdo_mapping_t *map; + const ec_pdo_t *pdo, *mapped_pdo; + + for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { + map = &fsm->slave->config->mapping[dir]; + + list_for_each_entry(pdo, &map->pdos, list) { + if (fsm->pdo) { + if (pdo == fsm->pdo) + fsm->pdo = NULL; + } else { + if ((mapped_pdo = ec_slave_find_pdo(fsm->slave, pdo->index))) + if (ec_pdo_equal_entries(pdo, mapped_pdo)) + continue; // Pdo configured correctly + + fsm->pdo = pdo; + goto configure; + } + } + } + + if (fsm->slave->master->debug_level) { + EC_DBG("No more Pdos to configure for slave %u.\n", + fsm->slave->ring_position); + } + fsm->state = ec_fsm_pdo_config_state_end; + return; + +configure: + if (fsm->slave->master->debug_level) { + EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n", + fsm->pdo->index, fsm->slave->ring_position); + } + + // set mapped PDO count to zero + fsm->sdodata.index = fsm->pdo->index; + fsm->sdodata.subindex = 0; // number of configured entries + EC_WRITE_U8(&fsm->sdo_value, 0); + fsm->sdodata.size = 1; + if (fsm->slave->master->debug_level) + EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n", + fsm->pdo->index); + + fsm->state = ec_fsm_pdo_config_state_zero_count; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + +/** Process next Pdo entry. + */ +ec_pdo_entry_t *ec_fsm_pdo_config_next_entry( + const ec_fsm_pdo_config_t *fsm, /**< Pdo configuration state machine */ + const struct list_head *list /**< current entry list item */ + ) +{ + list = list->next; + if (list == &fsm->pdo->entries) + return NULL; // no next entry + return list_entry(list, ec_pdo_entry_t, list); +} + +/*****************************************************************************/ + +/** Starts to add a Pdo entry. + */ +void ec_fsm_pdo_config_add_entry( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine. */ + ) +{ + uint32_t value; + + fsm->sdodata.subindex = fsm->entry_count; + value = fsm->entry->index << 16 + | fsm->entry->subindex << 8 | fsm->entry->bit_length; + EC_WRITE_U32(&fsm->sdo_value, value); + fsm->sdodata.size = 4; + + if (fsm->slave->master->debug_level) + EC_DBG("Configuring Pdo entry %08X at position %u.\n", + value, fsm->entry_count); + + fsm->state = ec_fsm_pdo_config_state_add_entry; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + +/** Set the number of configured entries to zero. + */ +void ec_fsm_pdo_config_state_zero_count( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to clear Pdo configuration for slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_pdo_config_state_error; + return; + } + + // find first entry + if (!(fsm->entry = + ec_fsm_pdo_config_next_entry(fsm, &fsm->pdo->entries))) { + if (fsm->slave->master->debug_level) + EC_DBG("No entries to configure for Pdo 0x%04X of slave %u.\n", + fsm->pdo->index, fsm->slave->ring_position); + ec_fsm_pdo_config_next_pdo(fsm); + return; + } + + // add first entry + fsm->entry_count = 1; + ec_fsm_pdo_config_add_entry(fsm); +} + +/*****************************************************************************/ + +/** Add a Pdo entry. + */ +void ec_fsm_pdo_config_state_add_entry( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to add entry 0x%04X:%u for slave %u.\n", + fsm->entry->index, fsm->entry->subindex, + fsm->slave->ring_position); + fsm->state = ec_fsm_pdo_config_state_error; + return; + } + + // find next entry + if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) { + // No more entries to add. Write entry count. + fsm->sdodata.subindex = 0; + EC_WRITE_U8(&fsm->sdo_value, fsm->entry_count); + fsm->sdodata.size = 1; + + if (fsm->slave->master->debug_level) + EC_DBG("Setting number of Pdo entries to %u.\n", + fsm->entry_count); + + fsm->state = ec_fsm_pdo_config_state_entry_count; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + return; + } + + // add next entry + fsm->entry_count++; + ec_fsm_pdo_config_add_entry(fsm); +} + +/*****************************************************************************/ + +/** Set the number of entries. + */ +void ec_fsm_pdo_config_state_entry_count( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to set number of entries for slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_pdo_config_state_error; + return; + } + + if (fsm->slave->master->debug_level) + EC_DBG("Successfully configured Pdo 0x%04X on slave %u.\n", + fsm->pdo->index, fsm->slave->ring_position); + + ec_fsm_pdo_config_next_pdo(fsm); +} + +/****************************************************************************** + * Common state functions + *****************************************************************************/ + +/** State: ERROR. + */ +void ec_fsm_pdo_config_state_error( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ +} + +/*****************************************************************************/ + +/** State: END. + */ +void ec_fsm_pdo_config_state_end( + ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */ + ) +{ +} + +/*****************************************************************************/ diff --git a/master/fsm_pdo_config.h b/master/fsm_pdo_config.h new file mode 100644 index 00000000..ecf089d3 --- /dev/null +++ b/master/fsm_pdo_config.h @@ -0,0 +1,83 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it + * and/or modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * The IgH EtherCAT Master is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * The right to use EtherCAT Technology is granted and comes free of + * charge under condition of compatibility of product made by + * Licensee. People intending to distribute/sell products based on the + * code, have to sign an agreement to guarantee that products using + * software based on IgH EtherCAT master stay compatible with the actual + * EtherCAT specification (which are released themselves as an open + * standard) as the (only) precondition to have the right to use EtherCAT + * Technology, IP and trade marks. + * + *****************************************************************************/ + +/** + \file + EtherCAT PDO configuration state machine structures. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_PDO_CONFIG__ +#define __EC_FSM_PDO_CONFIG__ + +#include "globals.h" +#include "../include/ecrt.h" +#include "datagram.h" +#include "fsm_coe.h" + +/*****************************************************************************/ + +/** + * \see ec_fsm_pdo_config + */ +typedef struct ec_fsm_pdo_config ec_fsm_pdo_config_t; + +/** Pdo configuration state machine. + */ +struct ec_fsm_pdo_config +{ + void (*state)(ec_fsm_pdo_config_t *); /**< state function */ + ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */ + ec_slave_t *slave; /**< Slave the FSM runs on. */ + + const ec_pdo_t *pdo; /**< Current PDO to configure. */ + + const ec_pdo_entry_t *entry; /**< Current entry. */ + ec_sdo_data_t sdodata; /**< SDO configuration data. */ + uint16_t sdo_value; /**< SDO value. */ + unsigned int entry_count; /**< Number of configured entries. */ +}; + +/*****************************************************************************/ + +void ec_fsm_pdo_config_init(ec_fsm_pdo_config_t *, ec_fsm_coe_t *); +void ec_fsm_pdo_config_clear(ec_fsm_pdo_config_t *); + +void ec_fsm_pdo_config_start(ec_fsm_pdo_config_t *, ec_slave_t *); +int ec_fsm_pdo_config_exec(ec_fsm_pdo_config_t *); +int ec_fsm_pdo_config_success(const ec_fsm_pdo_config_t *); + +/*****************************************************************************/ + +#endif diff --git a/master/fsm_slave.c b/master/fsm_slave.c index 83576751..b953898b 100644 --- a/master/fsm_slave.c +++ b/master/fsm_slave.c @@ -41,7 +41,6 @@ #include "globals.h" #include "master.h" #include "mailbox.h" -#include "fsm_mapping.h" #include "slave_config.h" #include "fsm_slave.h" @@ -62,6 +61,7 @@ void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_sdo_conf(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_mapping(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_pdo_conf(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *); @@ -69,10 +69,10 @@ void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *); -void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *); -void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_mapping(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *); void ec_fsm_slave_state_end(ec_fsm_slave_t *); @@ -80,10 +80,8 @@ void ec_fsm_slave_state_error(ec_fsm_slave_t *); /*****************************************************************************/ -/** - Constructor. -*/ - +/** Constructor. + */ void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */ ec_datagram_t *datagram /**< datagram structure to use */ ) @@ -95,14 +93,13 @@ void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */ ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe); + ec_fsm_pdo_config_init(&fsm->fsm_pdo, &fsm->fsm_coe); } /*****************************************************************************/ -/** - Destructor. -*/ - +/** Destructor. + */ void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */) { // clear sub state machines @@ -110,6 +107,7 @@ void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */) ec_fsm_change_clear(&fsm->fsm_change); ec_fsm_coe_clear(&fsm->fsm_coe); ec_fsm_mapping_clear(&fsm->fsm_map); + ec_fsm_pdo_config_clear(&fsm->fsm_pdo); } /*****************************************************************************/ @@ -884,7 +882,7 @@ void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *fsm /**< slave state machi return; } - // No CoE configuration to be applied? FIXME + // No CoE configuration to be applied? if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration ec_fsm_slave_conf_enter_mapping(fsm); return; @@ -941,14 +939,6 @@ void ec_fsm_slave_conf_enter_mapping( ec_fsm_slave_t *fsm /**< slave state machine */ ) { - ec_slave_t *slave = fsm->slave; - - if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { - // Slave does not support CoE: no configuration of PDO mapping. - ec_fsm_slave_conf_enter_pdo_sync(fsm); - return; - } - // start configuring PDO mapping fsm->state = ec_fsm_slave_conf_state_mapping; ec_fsm_mapping_start(&fsm->fsm_map, fsm->slave); @@ -975,6 +965,32 @@ void ec_fsm_slave_conf_state_mapping( return; } + // Start Pdo configuration + fsm->state = ec_fsm_slave_conf_state_pdo_conf; + ec_fsm_pdo_config_start(&fsm->fsm_pdo, fsm->slave); + ec_fsm_pdo_config_exec(&fsm->fsm_pdo); // execute immediately +} + +/*****************************************************************************/ + +/** + Slave configuration state: PDO_CONF. +*/ + +void ec_fsm_slave_conf_state_pdo_conf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + if (ec_fsm_pdo_config_exec(&fsm->fsm_pdo)) return; + + if (!ec_fsm_pdo_config_success(&fsm->fsm_pdo)) { + EC_ERR("Pdo configuration failed for slave %u.\n", + fsm->slave->ring_position); + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; + return; + } + ec_fsm_slave_conf_enter_pdo_sync(fsm); } diff --git a/master/fsm_slave.h b/master/fsm_slave.h index 16b0c7e2..a443f2ed 100644 --- a/master/fsm_slave.h +++ b/master/fsm_slave.h @@ -49,29 +49,29 @@ #include "fsm_change.h" #include "fsm_coe.h" #include "fsm_mapping.h" +#include "fsm_pdo_config.h" /*****************************************************************************/ typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */ -/** - Finite state machine of an EtherCAT slave. -*/ - +/** Finite state machine of an EtherCAT slave. + */ struct ec_fsm_slave { - ec_slave_t *slave; /**< slave the FSM runs on */ - ec_datagram_t *datagram; /**< datagram used in the state machine */ - unsigned int retries; /**< retries on datagram timeout. */ + ec_slave_t *slave; /**< Slave the FSM runs on. */ + ec_datagram_t *datagram; /**< Datagram used in the state machine. */ + unsigned int retries; /**< Retries on datagram timeout. */ - void (*state)(ec_fsm_slave_t *); /**< state function */ - ec_sdo_data_t *sdodata; /**< SDO configuration data */ - uint16_t sii_offset; /**< SII offset in words */ + void (*state)(ec_fsm_slave_t *); /**< State function. */ + ec_sdo_data_t *sdodata; /**< SDO configuration data. */ + uint16_t sii_offset; /**< SII offset in words. */ - ec_fsm_sii_t fsm_sii; /**< SII state machine */ - ec_fsm_change_t fsm_change; /**< State change state machine */ - ec_fsm_coe_t fsm_coe; /**< CoE state machine */ - ec_fsm_mapping_t fsm_map; /**< PDO mapping state machine */ + ec_fsm_sii_t fsm_sii; /**< SII state machine. */ + ec_fsm_change_t fsm_change; /**< State change state machine. */ + ec_fsm_coe_t fsm_coe; /**< CoE state machine. */ + ec_fsm_mapping_t fsm_map; /**< PDO mapping state machine. */ + ec_fsm_pdo_config_t fsm_pdo; /**< PDO configuration state machine. */ }; /*****************************************************************************/ diff --git a/master/pdo.c b/master/pdo.c index b4ee2388..65508119 100644 --- a/master/pdo.c +++ b/master/pdo.c @@ -170,6 +170,42 @@ int ec_pdo_copy_entries(ec_pdo_t *pdo, const ec_pdo_t *other) /*****************************************************************************/ +/** Compares the entries of two Pdos. + * + * \retval 1 The entries of the given Pdos are equal. + * \retval 0 The entries of the given Pdos differ. + */ +int ec_pdo_equal_entries( + const ec_pdo_t *pdo1, /**< First Pdo. */ + const ec_pdo_t *pdo2 /**< Second Pdo. */ + ) +{ + const struct list_head *head1, *head2, *item1, *item2; + const ec_pdo_entry_t *entry1, *entry2; + + head1 = item1 = &pdo1->entries; + head2 = item2 = &pdo2->entries; + + while (1) { + item1 = item1->next; + item2 = item2->next; + + if ((item1 == head1) ^ (item2 == head2)) // unequal lengths + return 0; + if (item1 == head1 && item2 == head2) // both finished + break; + + entry1 = list_entry(item1, ec_pdo_entry_t, list); + entry2 = list_entry(item2, ec_pdo_entry_t, list); + if (!ec_pdo_entry_equal(entry1, entry2)) + return 0; + } + + return 1; +} + +/*****************************************************************************/ + /** Pdo entry constructor. */ void ec_pdo_entry_init( @@ -237,3 +273,20 @@ int ec_pdo_entry_set_name( } /*****************************************************************************/ + +/** Compares two Pdo entries. + * + * \retval 1 The entries are equal. + * \retval 0 The entries differ. + */ +int ec_pdo_entry_equal( + const ec_pdo_entry_t *entry1, /**< First Pdo entry. */ + const ec_pdo_entry_t *entry2 /**< Second Pdo entry. */ + ) +{ + return entry1->index == entry2->index + && entry1->subindex == entry2->subindex + && entry1->bit_length == entry2->bit_length; +} + +/*****************************************************************************/ diff --git a/master/pdo.h b/master/pdo.h index 1ab8d983..3c6a70e4 100644 --- a/master/pdo.h +++ b/master/pdo.h @@ -79,11 +79,13 @@ int ec_pdo_init_copy(ec_pdo_t *, const ec_pdo_t *); void ec_pdo_clear(ec_pdo_t *); int ec_pdo_set_name(ec_pdo_t *, const char *); int ec_pdo_copy_entries(ec_pdo_t *, const ec_pdo_t *); +int ec_pdo_equal_entries(const ec_pdo_t *, const ec_pdo_t *); void ec_pdo_entry_init(ec_pdo_entry_t *); int ec_pdo_entry_init_copy(ec_pdo_entry_t *, const ec_pdo_entry_t *); void ec_pdo_entry_clear(ec_pdo_entry_t *); int ec_pdo_entry_set_name(ec_pdo_entry_t *, const char *); +int ec_pdo_entry_equal(const ec_pdo_entry_t *, const ec_pdo_entry_t *); /*****************************************************************************/ diff --git a/master/pdo_mapping.c b/master/pdo_mapping.c index c0ae6dee..8c43fd42 100644 --- a/master/pdo_mapping.c +++ b/master/pdo_mapping.c @@ -317,3 +317,23 @@ int ec_pdo_mapping_equal( } /*****************************************************************************/ + +/** Finds a Pdo with the given index. + */ +const ec_pdo_t *ec_pdo_mapping_find_pdo( + const ec_pdo_mapping_t *pm, /**< Pdo mapping. */ + uint16_t index /**< Pdo index. */ + ) +{ + ec_pdo_t *pdo; + + list_for_each_entry(pdo, &pm->pdos, list) { + if (pdo->index != index) + continue; + return pdo; + } + + return NULL; +} + +/*****************************************************************************/ diff --git a/master/pdo_mapping.h b/master/pdo_mapping.h index e9a973cd..fe8e89ef 100644 --- a/master/pdo_mapping.h +++ b/master/pdo_mapping.h @@ -72,6 +72,8 @@ int ec_pdo_mapping_copy(ec_pdo_mapping_t *, const ec_pdo_mapping_t *); uint16_t ec_pdo_mapping_total_size(const ec_pdo_mapping_t *); int ec_pdo_mapping_equal(const ec_pdo_mapping_t *, const ec_pdo_mapping_t *); +const ec_pdo_t *ec_pdo_mapping_find_pdo(const ec_pdo_mapping_t *, uint16_t); + /*****************************************************************************/ #endif diff --git a/master/slave.c b/master/slave.c index 4997de3c..ef68a9df 100644 --- a/master/slave.c +++ b/master/slave.c @@ -1189,7 +1189,7 @@ void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */ /** * Get an SDO from the dictionary. - * \returns The desired SDO, of NULL. + * \returns The desired SDO, or NULL. */ ec_sdo_t *ec_slave_get_sdo( @@ -1208,3 +1208,29 @@ ec_sdo_t *ec_slave_get_sdo( } /*****************************************************************************/ + +/** Finds a mapped Pdo. + * \returns The desired Pdo object, or NULL. + */ +const ec_pdo_t *ec_slave_find_pdo( + const ec_slave_t *slave, /**< Slave. */ + uint16_t index /**< Pdo index to find. */ + ) +{ + unsigned int i; + const ec_sync_t *sync; + const ec_pdo_t *pdo; + + for (i = 0; i < slave->sii_sync_count; i++) { + sync = &slave->sii_syncs[i]; + + if (!(pdo = ec_pdo_mapping_find_pdo(&sync->mapping, index))) + continue; + + return pdo; + } + + return NULL; +} + +/*****************************************************************************/ diff --git a/master/slave.h b/master/slave.h index ca8157bd..b90f3a0c 100644 --- a/master/slave.h +++ b/master/slave.h @@ -183,6 +183,7 @@ int ec_slave_validate(const ec_slave_t *, uint32_t, uint32_t); void ec_slave_sdo_dict_info(const ec_slave_t *, unsigned int *, unsigned int *); ec_sdo_t *ec_slave_get_sdo(ec_slave_t *, uint16_t); +const ec_pdo_t *ec_slave_find_pdo(const ec_slave_t *, uint16_t); /*****************************************************************************/