# HG changeset patch # User Florian Pose # Date 1190223868 0 # Node ID aece53f82df3852639fa0bbce5820110114662c9 # Parent 68d83b45dfa2bd53cbc4b0f986fa1f52aa096365 Read PDO mapping from SDO dictionary (working, not finished yet). diff -r 68d83b45dfa2 -r aece53f82df3 NEWS --- a/NEWS Wed Sep 19 17:32:39 2007 +0000 +++ b/NEWS Wed Sep 19 17:44:28 2007 +0000 @@ -6,6 +6,7 @@ Changes in version 1.3.2: +* Read dynamic PDO mapping from SDO dictionary. * Improved handling for spontaneous AL state changes. * Master takes mailbox sync manager configurations from EEPROM words 0x0018-0x001b, if no sync manager configurations are provided. diff -r 68d83b45dfa2 -r aece53f82df3 TODO --- a/TODO Wed Sep 19 17:32:39 2007 +0000 +++ b/TODO Wed Sep 19 17:44:28 2007 +0000 @@ -9,7 +9,6 @@ * Future features: - Interface/buffers for asynchronous domain IO. - Distributed clocks. - - Dynamic PDO mapping from SDO dictionary (see can-cia.org: cia301ds4). - Redundancy with 2 network adapters. - Mailbox handler - Support slaves, that don't support the LRW datagram, only LRD/LWR. diff -r 68d83b45dfa2 -r aece53f82df3 master/Kbuild.in --- a/master/Kbuild.in Wed Sep 19 17:32:39 2007 +0000 +++ b/master/Kbuild.in Wed Sep 19 17:44:28 2007 +0000 @@ -35,7 +35,7 @@ ec_master-objs := module.o master.o device.o pdo.o sync.o fmmu.o slave.o \ datagram.o domain.o mailbox.o canopen.o fsm_sii.o fsm_change.o \ - fsm_coe.o fsm_mapping.o fsm_slave.o fsm_master.o xmldev.o + fsm_coe.o fsm_coe_map.o fsm_mapping.o fsm_slave.o fsm_master.o xmldev.o ifeq (@ENABLE_EOE@,1) ec_master-objs += ethernet.o diff -r 68d83b45dfa2 -r aece53f82df3 master/fsm_coe_map.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_coe_map.c Wed Sep 19 17:44:28 2007 +0000 @@ -0,0 +1,421 @@ +/****************************************************************************** + * + * $Id: fsm_coe.c 920 2007-09-12 10:07:55Z fp $ + * + * 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 CoE mapping state machines. +*/ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "mailbox.h" +#include "canopen.h" +#include "fsm_coe_map.h" + +/*****************************************************************************/ + +void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *); + +void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *); + +void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *); + +/*****************************************************************************/ + +/** + Constructor. +*/ + +void ec_fsm_coe_map_init( + ec_fsm_coe_map_t *fsm, /**< finite state machine */ + ec_fsm_coe_t *fsm_coe /*< CoE state machine to use */ + ) +{ + fsm->state = NULL; + fsm->fsm_coe = fsm_coe; +} + +/*****************************************************************************/ + +/** + Destructor. +*/ + +void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */) +{ +} + +/*****************************************************************************/ + +/** + Starts to upload an SDO from a slave. +*/ + +void ec_fsm_coe_map_start( + ec_fsm_coe_map_t *fsm, /**< finite state machine */ + ec_slave_t *slave /**< EtherCAT slave */ + ) +{ + fsm->slave = slave; + fsm->state = ec_fsm_coe_map_state_start; +} + +/*****************************************************************************/ + +/** + Executes the current state of the state machine. + \return false, if state machine has terminated +*/ + +int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */) +{ + fsm->state(fsm); + + return fsm->state != ec_fsm_coe_map_state_end + && fsm->state != ec_fsm_coe_map_state_error; +} + +/*****************************************************************************/ + +/** + Returns, if the state machine terminated with success. + \return non-zero if successful. +*/ + +int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */) +{ + return fsm->state == ec_fsm_coe_map_state_end; +} + +/****************************************************************************** + * CoE dictionary state machine + *****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *fsm /**< finite state machine */) +{ + // read mapping of first sync manager + fsm->sync_index = 0; + ec_fsm_coe_map_action_next_sync(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_action_next_sync( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_sdo_entry_t *entry; + + for (; fsm->sync_index < 4; fsm->sync_index++) { + if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) + continue; + + if (slave->master->debug_level) + EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n", + fsm->sync_index, slave->ring_position); + + if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { + EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", + fsm->sync_sdo->index, + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + ec_sdo_request_init_read(&fsm->request, entry); + fsm->state = ec_fsm_coe_map_state_pdo_count; + ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + return; + } + + fsm->state = ec_fsm_coe_map_state_end; +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_state_pdo_count( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to read number of mapped PDOs from slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + fsm->sync_subindices = EC_READ_U8(fsm->request.data); + + if (fsm->slave->master->debug_level) + EC_DBG(" %u PDOs mapped.\n", fsm->sync_subindices); + + // read first PDO + fsm->sync_subindex = 1; + ec_fsm_coe_map_action_next_pdo(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_action_next_pdo( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + ec_sdo_entry_t *entry; + + if (fsm->sync_subindex <= fsm->sync_subindices) { + if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, + fsm->sync_subindex))) { + EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n", + fsm->sync_sdo->index, + fsm->sync_subindex, + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + ec_sdo_request_init_read(&fsm->request, entry); + fsm->state = ec_fsm_coe_map_state_pdo; + ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + return; + } + + // next sync manager + fsm->sync_index++; + ec_fsm_coe_map_action_next_sync(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_state_pdo( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to read mapped PDO index from slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + { + uint16_t pdo_index = EC_READ_U16(fsm->request.data); + ec_sdo_entry_t *entry; + + if (fsm->slave->master->debug_level) + EC_DBG(" PDO 0x%04X.\n", pdo_index); + + if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, pdo_index))) { + EC_ERR("Slave %u has no SDO 0x%04X.\n", + fsm->slave->ring_position, pdo_index); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { + EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", + fsm->pdo_sdo->index, + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + ec_sdo_request_init_read(&fsm->request, entry); + fsm->state = ec_fsm_coe_map_state_pdo_entry_count; + ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + return; + } +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_state_pdo_entry_count( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to read number of mapped PDO entries from slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + fsm->pdo_subindices = EC_READ_U8(fsm->request.data); + + if (fsm->slave->master->debug_level) + EC_DBG(" %u PDO entries mapped.\n", fsm->pdo_subindices); + + // read first PDO entry + fsm->pdo_subindex = 1; + ec_fsm_coe_map_action_next_pdo_entry(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_action_next_pdo_entry( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + ec_sdo_entry_t *entry; + + if (fsm->pdo_subindex <= fsm->pdo_subindices) { + if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, + fsm->pdo_subindex))) { + EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n", + fsm->pdo_sdo->index, fsm->pdo_subindex, + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + ec_sdo_request_init_read(&fsm->request, entry); + fsm->state = ec_fsm_coe_map_state_pdo_entry; + ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + return; + } + + // next PDO + fsm->sync_subindex++; + ec_fsm_coe_map_action_next_pdo(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_state_pdo_entry( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to read index of mapped PDO entry from slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + { + uint32_t pdo_entry_info; + uint16_t pdo_entry_index; + ec_sdo_t *sdo; + + pdo_entry_info = EC_READ_U32(fsm->request.data); + pdo_entry_index = pdo_entry_info >> 16; + + if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry_index))) { + EC_ERR("Slave %u has no SDO 0x%04X.\n", + fsm->slave->ring_position, pdo_entry_index); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + if (fsm->slave->master->debug_level) { + size_t bitsize = pdo_entry_info & 0xFFFF; + EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", + pdo_entry_index, sdo->name, bitsize); + } + + // next PDO entry + fsm->pdo_subindex++; + ec_fsm_coe_map_action_next_pdo_entry(fsm); + } +} + +/*****************************************************************************/ + +/** + State: ERROR. +*/ + +void ec_fsm_coe_map_state_error( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ +} + +/*****************************************************************************/ + +/** + State: END. +*/ + +void ec_fsm_coe_map_state_end( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ +} + +/*****************************************************************************/ diff -r 68d83b45dfa2 -r aece53f82df3 master/fsm_coe_map.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_coe_map.h Wed Sep 19 17:44:28 2007 +0000 @@ -0,0 +1,86 @@ +/****************************************************************************** + * + * $Id: fsm_coe.h 702 2006-12-18 18:10:52Z fp $ + * + * 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 CoE mapping state machines. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_COE_MAP__ +#define __EC_FSM_COE_MAP__ + +#include "globals.h" +#include "datagram.h" +#include "slave.h" +#include "fsm_coe.h" + +/*****************************************************************************/ + +typedef struct ec_fsm_coe_map ec_fsm_coe_map_t; /**< \see ec_fsm_coe_map */ + +/** + */ + +struct ec_fsm_coe_map +{ + void (*state)(ec_fsm_coe_map_t *); /**< CoE mapping state function */ + ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */ + + ec_slave_t *slave; /**< EtherCAT slave */ + ec_sdo_request_t request; + + unsigned int sync_index; + ec_sdo_t *sync_sdo; + uint8_t sync_subindices; + uint16_t sync_subindex; + + ec_sdo_t *pdo_sdo; + uint8_t pdo_subindices; + uint16_t pdo_subindex; +}; + +/*****************************************************************************/ + +void ec_fsm_coe_map_init(ec_fsm_coe_map_t *, ec_fsm_coe_t *); +void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *); + +void ec_fsm_coe_map_start(ec_fsm_coe_map_t *, ec_slave_t *); + +int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *); +int ec_fsm_coe_map_success(ec_fsm_coe_map_t *); + +/*****************************************************************************/ + +#endif diff -r 68d83b45dfa2 -r aece53f82df3 master/fsm_master.c --- a/master/fsm_master.c Wed Sep 19 17:32:39 2007 +0000 +++ b/master/fsm_master.c Wed Sep 19 17:44:28 2007 +0000 @@ -60,6 +60,7 @@ void ec_fsm_master_state_scan_slaves(ec_fsm_master_t *); void ec_fsm_master_state_write_eeprom(ec_fsm_master_t *); void ec_fsm_master_state_sdodict(ec_fsm_master_t *); +void ec_fsm_master_state_pdomap(ec_fsm_master_t *); void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); void ec_fsm_master_state_end(ec_fsm_master_t *); void ec_fsm_master_state_error(ec_fsm_master_t *); @@ -90,6 +91,7 @@ ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); + ec_fsm_coe_map_init(&fsm->fsm_coe_map, &fsm->fsm_coe); } /*****************************************************************************/ @@ -105,6 +107,7 @@ ec_fsm_sii_clear(&fsm->fsm_sii); ec_fsm_change_clear(&fsm->fsm_change); ec_fsm_coe_clear(&fsm->fsm_coe); + ec_fsm_coe_map_clear(&fsm->fsm_coe_map); } /*****************************************************************************/ @@ -521,6 +524,29 @@ return; } + // check, if slaves have their PDO mapping to be read. + list_for_each_entry(slave, &master->slaves, list) { + if (!(slave->sii_mailbox_protocols & EC_MBOX_COE) + || slave->pdo_mapping_fetched + || !slave->sdo_dictionary_fetched + || slave->current_state == EC_SLAVE_STATE_INIT + || slave->online_state == EC_SLAVE_OFFLINE) continue; + + if (master->debug_level) { + EC_DBG("Fetching PDO mapping from slave %i via CoE.\n", + slave->ring_position); + } + + slave->pdo_mapping_fetched = 1; + + // start fetching PDO mapping + fsm->idle = 0; + fsm->state = ec_fsm_master_state_pdomap; + ec_fsm_coe_map_start(&fsm->fsm_coe_map, slave); + ec_fsm_coe_map_exec(&fsm->fsm_coe_map); // execute immediately + return; + } + // check for pending EEPROM write operations. if (ec_fsm_master_action_process_eeprom(fsm)) return; // EEPROM write request found @@ -1003,6 +1029,26 @@ /*****************************************************************************/ /** + */ + +void ec_fsm_master_state_pdomap( + ec_fsm_master_t *fsm /**< master state machine */ + ) +{ + if (ec_fsm_coe_map_exec(&fsm->fsm_coe_map)) return; + + if (!ec_fsm_coe_map_success(&fsm->fsm_coe_map)) { + fsm->state = ec_fsm_master_state_error; + return; + } + + // fetching of PDO mapping finished + fsm->state = ec_fsm_master_state_end; +} + +/*****************************************************************************/ + +/** Master state: SDO REQUEST. */ diff -r 68d83b45dfa2 -r aece53f82df3 master/fsm_master.h --- a/master/fsm_master.h Wed Sep 19 17:32:39 2007 +0000 +++ b/master/fsm_master.h Wed Sep 19 17:44:28 2007 +0000 @@ -48,6 +48,7 @@ #include "canopen.h" #include "fsm_slave.h" +#include "fsm_coe_map.h" /*****************************************************************************/ @@ -99,6 +100,7 @@ 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_coe_map_t fsm_coe_map; /**< CoE mapping state machine */ }; /*****************************************************************************/ diff -r 68d83b45dfa2 -r aece53f82df3 master/slave.c --- a/master/slave.c Wed Sep 19 17:32:39 2007 +0000 +++ b/master/slave.c Wed Sep 19 17:44:28 2007 +0000 @@ -154,6 +154,7 @@ INIT_LIST_HEAD(&slave->sdo_confs); slave->sdo_dictionary_fetched = 0; + slave->pdo_mapping_fetched = 0; slave->jiffies_preop = 0; for (i = 0; i < 4; i++) { diff -r 68d83b45dfa2 -r aece53f82df3 master/slave.h --- a/master/slave.h Wed Sep 19 17:32:39 2007 +0000 +++ b/master/slave.h Wed Sep 19 17:44:28 2007 +0000 @@ -175,6 +175,7 @@ struct list_head sdo_dictionary; /**< SDO dictionary list */ struct list_head sdo_confs; /**< list of SDO configurations */ uint8_t sdo_dictionary_fetched; /**< dictionary has been fetched */ + uint8_t pdo_mapping_fetched; /**< PDO mapping has been fetched */ unsigned long jiffies_preop; /**< time, the slave went to PREOP */ };