bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Major Pdo configuration change: Read mapping of unknown Pdos during 

configuration.
This commit is contained in:
Florian Pose 2008-07-31 16:13:29 +00:00
parent 60aa049b63
commit 5317b27ff9
26 changed files with 1539 additions and 1558 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
TODO
View File

@ -8,12 +8,14 @@ $Id$
Version 1.4.0:
* Read Pdo mapping for unknown Pdos before configuring them.
* Attach Pdo names from SII or Coe dictioary to Pdos read via CoE.
* Update documentation.
* Get original driver for r8169.
* Race in jiffies frame timeout?
* Error at no sync manager for configuration.
* Remove ec_sync_t::assign_source?
* Repair rcethercat status.
* Debug output for all configuration functions of the application interface.
* Output warning, if there is no sync manager for a configuration.
* Abort code messages in userspace.
Future issues:
@ -37,12 +39,12 @@ Future issues:
- Add a -n (numeric) switch.
- Check for unwanted options.
* Segmented Sdo downloads.
* Get original driver for r8169.
Smaller issues:
* Stop master thread at ecrt_master_request() to get rid of allow_* flags?
* Evaluate SII Size field (0x003E) to determine maximum SII memory size.
* Unite fsm_pdo_mapping, fsm_pdo_config and fsm_coe_map.
* Clear sync managers in INIT.
* Read out CRC counters.
* Configure slave ports to automatically open on link detection.

7
documentation/graphs/Makefile
View File

@ -5,10 +5,11 @@
#-----------------------------------------------------------------------------
GRAPHS := \
fsm_coe_map \
fsm_master \
fsm_pdo_assign \
fsm_pdo_mapping \
fsm_pdo_conf \
fsm_pdo_read \
fsm_pdo_entry_conf \
fsm_pdo_entry_read \
fsm_slave_conf \
fsm_slave_scan

33
documentation/graphs/fsm_coe_map.dot
View File

@ -1,33 +0,0 @@
/* $Id$ */
digraph readpdomapping {
size="7,9"
center=1
ratio=fill
next_sync [shape=point,label=""]
next_pdo [shape=point,label=""]
next_pdo_entry [shape=point,label=""]
start -> next_sync [label="first SM"]
next_sync -> pdo_count [weight=5]
next_sync -> end
pdo_count -> next_pdo [weight=5]
next_pdo -> pdo [weight=5]
next_pdo -> next_sync
pdo -> pdo_entry_count [weight=5]
pdo_entry_count -> next_pdo_entry [weight=10]
next_pdo_entry -> pdo_entry [weight=5]
next_pdo_entry -> next_pdo
pdo_entry -> next_pdo_entry
end
}

19
documentation/graphs/fsm_pdo_assign.dot
View File

@ -1,19 +0,0 @@
/* $Id$ */
digraph pdomapping {
size="7,9"
center=1
ratio=fill
next_sync [shape=point,label=""]
start -> next_sync [label="First SM"]
next_sync -> end [label="No more SMs"]
next_sync -> zero_count [label="Next SM"]
zero_count -> next_sync [label="No Pdos"]
zero_count -> add_pdo [label="Map first Pdo", weight=5]
add_pdo -> add_pdo [label="Map next Pdo"]
add_pdo -> pdo_count [label="No more Pdos", weight=5]
pdo_count -> next_sync
}

54
documentation/graphs/fsm_pdo_conf.dot Normal file
View File

@ -0,0 +1,54 @@
/* $Id$ */
digraph pdo_conf {
size="7,9"
center=1
ratio=fill
start [fontname="Helvetica"]
start -> action_next_sync [fontname="Helvetica",label="First SM",weight=10]
start -> end [fontname="Helvetica",label="No config"]
action_next_sync [shape=point,label=""]
action_next_sync -> action_check_assignment [fontname="Helvetica",label="No Pdos"]
action_next_sync -> action_pdo_mapping [fontname="Helvetica",label="First Pdo",weight=10]
action_next_sync -> end [fontname="Helvetica",label="No more SMs"]
action_pdo_mapping [shape=point,label=""]
action_pdo_mapping -> read_mapping [fontname="Helvetica",label="Unknown"]
action_pdo_mapping -> action_check_mapping [weight=10]
read_mapping [fontname="Helvetica"]
read_mapping -> action_check_mapping
action_check_mapping [shape=point,label=""]
action_check_mapping -> action_next_pdo_mapping
action_check_mapping -> mapping [weight=10]
mapping [fontname="Helvetica"]
mapping -> action_next_pdo_mapping [weight=10]
action_next_pdo_mapping [shape=point,label=""]
action_next_pdo_mapping -> action_check_assignment [weight=10]
action_next_pdo_mapping -> action_pdo_mapping
[fontname="Helvetica",label="Next Pdo"]
action_check_assignment [shape=point,label=""]
action_check_assignment -> action_next_sync [fontname="Helvetica",label="Assign ok"]
action_check_assignment -> zero_pdo_count [weight=10]
zero_pdo_count [fontname="Helvetica"]
zero_pdo_count -> action_next_sync [fontname="Helvetica",label="No Pdos"]
zero_pdo_count -> action_assign_pdo [fontname="Helvetica",label="First Pdo", weight=10]
action_assign_pdo [shape=point,label=""]
action_assign_pdo -> assign_pdo [weight=10]
assign_pdo [fontname="Helvetica"]
assign_pdo -> set_pdo_count [fontname="Helvetica",label="No more Pdos", weight=10]
assign_pdo -> action_assign_pdo [fontname="Helvetica",label="Next Pdo"]
set_pdo_count [fontname="Helvetica"]
set_pdo_count -> action_next_sync
}

28
documentation/graphs/fsm_pdo_entry_conf.dot Normal file
View File

@ -0,0 +1,28 @@
/* $Id$ */
digraph pdo_entry_conf {
size="7,9"
center=1
ratio=fill
start [fontname="Helvetica"]
start -> zero_entry_count [weight=10]
zero_entry_count [fontname="Helvetica"]
zero_entry_count -> end [fontname="Helvetica",label="No Entries"]
zero_entry_count -> action_map [fontname="Helvetica",label="Add first entry", weight=10]
action_map [shape=point,label=""]
action_map -> map_entry [weight=10]
map_entry [fontname="Helvetica"]
map_entry -> action_map [fontname="Helvetica",label="Next entry"]
map_entry -> set_entry_count [fontname="Helvetica",label="No more Entries", weight=10]
set_entry_count [fontname="Helvetica"]
set_entry_count -> end [weight=10]
end [fontname="Helvetica"]
end
}

23
documentation/graphs/fsm_pdo_entry_read.dot Normal file
View File

@ -0,0 +1,23 @@
/* $Id$ */
digraph pdo_entry_read {
size="7,9"
center=1
ratio=fill
start [fontname="Helvetica"]
start -> count [weight=5]
count [fontname="Helvetica"]
count -> action_next [weight=5]
action_next [shape=point,label=""]
action_next -> pdo_entry [fontname="Helvetica", label="Next entry", weight=5]
action_next -> end [fontname="Helvetica", label="No more entries"]
pdo_entry [fontname="Helvetica"]
pdo_entry -> action_next
end [fontname="Helvetica"]
}

19
documentation/graphs/fsm_pdo_mapping.dot
View File

@ -1,19 +0,0 @@
/* $Id$ */
digraph pdoconfig {
size="7,9"
center=1
ratio=fill
next_pdo [shape=point,label=""]
start -> next_pdo [label="First Pdo"]
next_pdo -> end [label="No more Pdos"]
next_pdo -> zero_count [label="Next Pdo"]
zero_count -> next_pdo [label="No Entries"]
zero_count -> add_entry [label="Add first entry", weight=5]
add_entry -> add_entry [label="Add next entry"]
add_entry -> entry_count [label="No more Entries", weight=5]
entry_count -> next_pdo
}

30
documentation/graphs/fsm_pdo_read.dot Normal file
View File

@ -0,0 +1,30 @@
/* $Id$ */
digraph pdo_read {
size="7,9"
center=1
ratio=fill
start [fontname="Helvetica"]
start -> action_next_sync [fontname="Helvetica", label="First SM", weight=5]
action_next_sync [shape=point,label=""]
action_next_sync -> pdo_count [weight=5]
action_next_sync -> end
pdo_count [fontname="Helvetica"]
pdo_count -> action_next_pdo [weight=5]
action_next_pdo [shape=point,label=""]
action_next_pdo -> pdo [fontname="Helvetica", label="Next Pdo", weight=5]
action_next_pdo -> action_next_sync [fontname="Helvetica", label="No more Pdos"]
pdo [fontname="Helvetica"]
pdo -> pdo_entries [weight=5]
pdo_entries [fontname="Helvetica"]
pdo_entries -> action_next_pdo
end [fontname="Helvetica"]
}

15
documentation/graphs/fsm_slave_conf.dot
View File

@ -53,17 +53,12 @@ digraph slaveconf {
pdo_sync [fontname="Helvetica"]
//pdo_sync -> pdo_sync
//pdo_sync -> error
pdo_sync -> pdo_mapping [weight=10]
pdo_sync -> pdo_conf [weight=10]
pdo_mapping [fontname="Helvetica"]
//pdo_mapping -> pdo_mapping
//pdo_mapping -> error
pdo_mapping -> pdo_assign [weight=10]
pdo_assign [fontname="Helvetica"]
//pdo_assign -> pdo_assign
//pdo_assign -> error
pdo_assign -> enter_fmmu [weight=10]
pdo_conf [fontname="Helvetica"]
//pdo_conf -> pdo_conf
//pdo_conf -> error
pdo_conf -> enter_fmmu [weight=10]
enter_fmmu [shape=point,label=""]
enter_fmmu -> enter_safeop [fontname="Helvetica",label="No FMMUs\nto configure"]

5
master/Kbuild.in
View File

@ -43,10 +43,9 @@ ec_master-objs := \
fmmu_config.o \
fsm_change.o \
fsm_coe.o \
fsm_coe_map.o \
fsm_master.o \
fsm_pdo_assign.o \
fsm_pdo_mapping.o \
fsm_pdo.o \
fsm_pdo_entry.o \
fsm_sii.o \
fsm_slave_config.o \
fsm_slave_scan.o \

5
master/Makefile.am
View File

@ -43,10 +43,9 @@ EXTRA_DIST = \
fmmu_config.c fmmu_config.h \
fsm_change.c fsm_change.h \
fsm_coe.c fsm_coe.h \
fsm_coe_map.c fsm_coe_map.h \
fsm_master.c fsm_master.h \
fsm_pdo_assign.c fsm_pdo_assign.h \
fsm_pdo_mapping.c fsm_pdo_mapping.h \
fsm_pdo.c fsm_pdo.h \
fsm_pdo_entry.c fsm_pdo_entry.h \
fsm_sii.c fsm_sii.h \
fsm_slave_config.c fsm_slave_config.h \
fsm_slave_scan.c fsm_slave_scan.h \

460
master/fsm_coe_map.c
View File

@ -1,460 +0,0 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it
* and/or modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* The IgH EtherCAT Master is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* The right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
/**
\file
EtherCAT Pdo information state machines.
*/
/*****************************************************************************/
#include "globals.h"
#include "master.h"
#include "mailbox.h"
#include "sdo_request.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->fsm_coe = fsm_coe;
fsm->state = NULL;
ec_sdo_request_init(&fsm->request);
ec_pdo_list_init(&fsm->pdos);
}
/*****************************************************************************/
/**
Destructor.
*/
void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)
{
ec_sdo_request_clear(&fsm->request);
ec_pdo_list_clear(&fsm->pdos);
}
/*****************************************************************************/
/**
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;
}
/******************************************************************************
* state functions
*****************************************************************************/
/** Start reading Pdo assignment.
*/
void ec_fsm_coe_map_state_start(
ec_fsm_coe_map_t *fsm /**< finite state machine */
)
{
// read Pdo assignment for first sync manager not reserved for mailbox
fsm->sync_index = 1; // next is 2
ec_fsm_coe_map_action_next_sync(fsm);
}
/*****************************************************************************/
/** Read Pdo assignment of next sync manager.
*/
void ec_fsm_coe_map_action_next_sync(
ec_fsm_coe_map_t *fsm /**< Finite state machine */
)
{
ec_slave_t *slave = fsm->slave;
fsm->sync_index++;
for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))
continue;
fsm->sync_sdo_index = 0x1C10 + fsm->sync_index;
if (slave->master->debug_level)
EC_DBG("Reading Pdo assignment of SM%u of slave %u.\n",
fsm->sync_index, slave->ring_position);
ec_pdo_list_clear_pdos(&fsm->pdos);
ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_coe_map_state_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
if (slave->master->debug_level)
EC_DBG("Reading of Pdo assignment finished for slave %u.\n",
slave->ring_position);
fsm->state = ec_fsm_coe_map_state_end;
}
/*****************************************************************************/
/**
* Count assigned Pdos.
*/
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 assigned Pdos from slave %u.\n",
fsm->slave->ring_position);
fsm->state = ec_fsm_coe_map_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint8_t)) {
EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "
"from slave %u.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex,
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 assigned.\n", fsm->sync_subindices);
// read first Pdo
fsm->sync_subindex = 1;
ec_fsm_coe_map_action_next_pdo(fsm);
}
/*****************************************************************************/
/**
* Read next Pdo.
*/
void ec_fsm_coe_map_action_next_pdo(
ec_fsm_coe_map_t *fsm /**< finite state machine */
)
{
if (fsm->sync_subindex <= fsm->sync_subindices) {
ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index,
fsm->sync_subindex);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_coe_map_state_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// finished reading Pdo assignment/mapping
if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {
fsm->state = ec_fsm_coe_map_state_error;
return;
}
fsm->sync->assign_source = EC_ASSIGN_COE;
ec_pdo_list_clear_pdos(&fsm->pdos);
// next sync manager
ec_fsm_coe_map_action_next_sync(fsm);
}
/*****************************************************************************/
/**
* Fetch Pdo information.
*/
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 assigned Pdo index from slave %u.\n",
fsm->slave->ring_position);
fsm->state = ec_fsm_coe_map_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint16_t)) {
EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "
"from slave %u.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex,
fsm->slave->ring_position);
fsm->state = ec_fsm_coe_map_state_error;
return;
}
if (!(fsm->pdo = (ec_pdo_t *)
kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate Pdo.\n");
fsm->state = ec_fsm_coe_map_state_error;
return;
}
ec_pdo_init(fsm->pdo);
fsm->pdo->index = EC_READ_U16(fsm->request.data);
fsm->pdo->sync_index = fsm->sync_index;
if (fsm->slave->master->debug_level)
EC_DBG("Pdo 0x%04X.\n", fsm->pdo->index);
list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/**
* Read number of mapped Pdo entries.
*/
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;
}
if (fsm->request.data_size != sizeof(uint8_t)) {
EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "
"from slave %u.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex,
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);
}
/*****************************************************************************/
/**
* Read next Pdo entry.
*/
void ec_fsm_coe_map_action_next_pdo_entry(
ec_fsm_coe_map_t *fsm /**< finite state machine */
)
{
if (fsm->pdo_subindex <= fsm->pdo_subindices) {
ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_coe_map_state_pdo_entry;
ec_fsm_coe_transfer(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);
}
/*****************************************************************************/
/**
* Read Pdo entry information.
*/
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 mapped Pdo entry from slave %u.\n",
fsm->slave->ring_position);
fsm->state = ec_fsm_coe_map_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint32_t)) {
EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "
"from slave %u.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex,
fsm->slave->ring_position);
fsm->state = ec_fsm_coe_map_state_error;
} else {
uint32_t pdo_entry_info;
ec_pdo_entry_t *pdo_entry;
pdo_entry_info = EC_READ_U32(fsm->request.data);
if (!(pdo_entry = (ec_pdo_entry_t *)
kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate Pdo entry.\n");
fsm->state = ec_fsm_coe_map_state_error;
return;
}
ec_pdo_entry_init(pdo_entry);
pdo_entry->index = pdo_entry_info >> 16;
pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;
pdo_entry->bit_length = pdo_entry_info & 0xFF;
if (!pdo_entry->index && !pdo_entry->subindex) {
if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {
ec_pdo_entry_clear(pdo_entry);
kfree(pdo_entry);
fsm->state = ec_fsm_coe_map_state_error;
return;
}
}
if (fsm->slave->master->debug_level) {
EC_DBG("Pdo entry 0x%04X:%02X, %u bit, \"%s\".\n",
pdo_entry->index, pdo_entry->subindex,
pdo_entry->bit_length,
pdo_entry->name ? pdo_entry->name : "???");
}
// next Pdo entry
list_add_tail(&pdo_entry->list, &fsm->pdo->entries);
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 */
)
{
}
/*****************************************************************************/

89
master/fsm_coe_map.h
View File

@ -1,89 +0,0 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it
* and/or modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* The IgH EtherCAT Master is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* The right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
/**
\file
EtherCAT CoE mapping state machines.
*/
/*****************************************************************************/
#ifndef __EC_FSM_COE_MAP_H__
#define __EC_FSM_COE_MAP_H__
#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 */
/**
* \todo doc
*/
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; /**< Sdo request */
uint8_t sync_index; /**< Index of the current sync manager. */
ec_sync_t *sync; /**< Pdo sync manager. */
uint16_t sync_sdo_index; /**< Index of the mapping Sdo. */
uint8_t sync_subindices; /**< number of mapped Pdos */
uint16_t sync_subindex; /**< current subindex in mapping Sdo */
ec_pdo_list_t pdos; /**< List of read in Pdos. */
ec_pdo_t *pdo; /**< Current Pdo. */
ec_sdo_t *pdo_sdo; /**< Current Pdo Sdo. */
uint8_t pdo_subindices; /**< Number of Pdo entries. */
uint16_t pdo_subindex; /**< Current subindex in Pdo Sdo. */
};
/*****************************************************************************/
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

19
master/fsm_master.c
View File

@ -79,13 +79,14 @@ void ec_fsm_master_init(
fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;
// init sub-state-machines
ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram);
ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram,
&fsm->fsm_slave_config, &fsm->fsm_coe_map);
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);
ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);
ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,
&fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_pdo);
ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram,
&fsm->fsm_slave_config, &fsm->fsm_pdo);
ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);
}
/*****************************************************************************/
@ -97,12 +98,12 @@ void ec_fsm_master_clear(
)
{
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_fsm_pdo_clear(&fsm->fsm_pdo);
ec_fsm_change_clear(&fsm->fsm_change);
ec_fsm_slave_config_clear(&fsm->fsm_slave_config);
ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan);
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);
}
/*****************************************************************************/

8
master/fsm_master.h
View File

@ -48,7 +48,7 @@
#include "sdo_request.h"
#include "fsm_slave_config.h"
#include "fsm_slave_scan.h"
#include "fsm_coe_map.h"
#include "fsm_pdo.h"
/*****************************************************************************/
@ -95,12 +95,12 @@ struct ec_fsm_master {
off_t sii_index; /**< index to SII write request data */
ec_sdo_request_t *sdo_request; /**< Sdo request to process. */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
ec_fsm_pdo_t fsm_pdo; /**< Pdo configuration state machine. */
ec_fsm_change_t fsm_change; /**< State change state machine */
ec_fsm_slave_config_t fsm_slave_config; /**< slave state machine */
ec_fsm_slave_scan_t fsm_slave_scan; /**< slave state machine */
ec_fsm_sii_t fsm_sii; /**< SII state machine */
ec_fsm_change_t fsm_change; /**< State change state machine */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
ec_fsm_coe_map_t fsm_coe_map; /**< CoE mapping state machine */
};
/*****************************************************************************/

762
master/fsm_pdo.c Normal file
View File

@ -0,0 +1,762 @@
/******************************************************************************
*
* $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.h"
/*****************************************************************************/
void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);
void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_state_entries(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);
void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);
void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);
void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);
/*****************************************************************************/
/** Constructor.
*/
void ec_fsm_pdo_init(
ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */
ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */
)
{
fsm->fsm_coe = fsm_coe;
ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);
ec_pdo_list_init(&fsm->pdos);
ec_sdo_request_init(&fsm->request);
ec_pdo_init(&fsm->slave_pdo);
}
/*****************************************************************************/
/** Destructor.
*/
void ec_fsm_pdo_clear(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);
ec_pdo_list_clear(&fsm->pdos);
ec_sdo_request_clear(&fsm->request);
ec_pdo_clear(&fsm->slave_pdo);
}
/*****************************************************************************/
/** Start reading the Pdo configuration.
*/
void ec_fsm_pdo_start_reading(
ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */
ec_slave_t *slave /**< slave to configure */
)
{
fsm->slave = slave;
fsm->state = ec_fsm_pdo_read_state_start;
}
/*****************************************************************************/
/** Start writing the Pdo configuration.
*/
void ec_fsm_pdo_start_configuration(
ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */
ec_slave_t *slave /**< slave to configure */
)
{
fsm->slave = slave;
fsm->state = ec_fsm_pdo_conf_state_start;
}
/*****************************************************************************/
/** Get running state.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_running(
const ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
return fsm->state != ec_fsm_pdo_state_end
&& fsm->state != ec_fsm_pdo_state_error;
}
/*****************************************************************************/
/** Executes the current state of the state machine.
*
* If the state machine's datagram is not sent or received yet, the execution
* of the state machine is delayed to the next cycle.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_exec(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
fsm->state(fsm);
return ec_fsm_pdo_running(fsm);
}
/*****************************************************************************/
/** Get execution result.
*
* \return true, if the state machine terminated gracefully
*/
int ec_fsm_pdo_success(
const ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
return fsm->state == ec_fsm_pdo_state_end;
}
/******************************************************************************
* Reading state funtions.
*****************************************************************************/
/** Start reading Pdo assignment.
*/
void ec_fsm_pdo_read_state_start(
ec_fsm_pdo_t *fsm /**< finite state machine */
)
{
// read Pdo assignment for first sync manager not reserved for mailbox
fsm->sync_index = 1; // next is 2
ec_fsm_pdo_read_action_next_sync(fsm);
}
/*****************************************************************************/
/** Read Pdo assignment of next sync manager.
*/
void ec_fsm_pdo_read_action_next_sync(
ec_fsm_pdo_t *fsm /**< Finite state machine */
)
{
ec_slave_t *slave = fsm->slave;
fsm->sync_index++;
for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))
continue;
if (slave->master->debug_level)
EC_DBG("Reading Pdo assignment of SM%u.\n", fsm->sync_index);
ec_pdo_list_clear_pdos(&fsm->pdos);
ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_read_state_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
if (slave->master->debug_level)
EC_DBG("Reading of Pdo configuration finished.\n");
fsm->state = ec_fsm_pdo_state_end;
}
/*****************************************************************************/
/** Count assigned Pdos.
*/
void ec_fsm_pdo_read_state_pdo_count(
ec_fsm_pdo_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 assigned Pdos for SM%u.\n",
fsm->sync_index);
fsm->state = ec_fsm_pdo_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint8_t)) {
EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "
"from slave %u.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex,
fsm->slave->ring_position);
fsm->state = ec_fsm_pdo_state_error;
return;
}
fsm->pdo_count = EC_READ_U8(fsm->request.data);
if (fsm->slave->master->debug_level)
EC_DBG("%u Pdos assigned.\n", fsm->pdo_count);
// read first Pdo
fsm->pdo_pos = 1;
ec_fsm_pdo_read_action_next_pdo(fsm);
}
/*****************************************************************************/
/** Read next Pdo.
*/
void ec_fsm_pdo_read_action_next_pdo(
ec_fsm_pdo_t *fsm /**< finite state machine */
)
{
if (fsm->pdo_pos <= fsm->pdo_count) {
ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index,
fsm->pdo_pos);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_read_state_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// finished reading Pdo configuration
if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {
fsm->state = ec_fsm_pdo_state_error;
return;
}
fsm->sync->assign_source = EC_ASSIGN_COE;
ec_pdo_list_clear_pdos(&fsm->pdos);
// next sync manager
ec_fsm_pdo_read_action_next_sync(fsm);
}
/*****************************************************************************/
/** Fetch Pdo information.
*/
void ec_fsm_pdo_read_state_pdo(
ec_fsm_pdo_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 assigned Pdo %u from SM%u.\n",
fsm->pdo_pos, fsm->sync_index);
fsm->state = ec_fsm_pdo_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint16_t)) {
EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "
"from slave %u.\n", fsm->request.data_size,
fsm->request.index, fsm->request.subindex,
fsm->slave->ring_position);
fsm->state = ec_fsm_pdo_state_error;
return;
}
if (!(fsm->pdo = (ec_pdo_t *)
kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate Pdo.\n");
fsm->state = ec_fsm_pdo_state_error;
return;
}
ec_pdo_init(fsm->pdo);
fsm->pdo->index = EC_READ_U16(fsm->request.data);
fsm->pdo->sync_index = fsm->sync_index;
if (fsm->slave->master->debug_level)
EC_DBG("Pdo 0x%04X.\n", fsm->pdo->index);
list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
fsm->state = ec_fsm_pdo_read_state_pdo_entries;
ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
fsm->state(fsm); // execute immediately
}
/*****************************************************************************/
/** Fetch Pdo information.
*/
void ec_fsm_pdo_read_state_pdo_entries(
ec_fsm_pdo_t *fsm /**< finite state machine */
)
{
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
return;
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
EC_ERR("Failed to read mapped Pdo entries for Pdo 0x%04X.\n",
fsm->pdo->index);
fsm->state = ec_fsm_pdo_state_error;
return;
}
// next Pdo
fsm->pdo_pos++;
ec_fsm_pdo_read_action_next_pdo(fsm);
}
/******************************************************************************
* Writing state functions.
*****************************************************************************/
/** Start Pdo configuration.
*/
void ec_fsm_pdo_conf_state_start(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (!fsm->slave->config) {
fsm->state = ec_fsm_pdo_state_end;
return;
}
fsm->sync_index = 0xff; // next is zero
ec_fsm_pdo_conf_action_next_sync(fsm);
}
/*****************************************************************************/
/** Assign next Pdo.
*/
ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(
const ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */
const struct list_head *list /**< current Pdo list item */
)
{
list = list->next;
if (list == &fsm->pdos.list)
return NULL; // no next Pdo
return list_entry(list, ec_pdo_t, list);
}
/*****************************************************************************/
/** Get the next sync manager for a pdo configuration.
*/
void ec_fsm_pdo_conf_action_next_sync(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
fsm->sync_index++;
for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
if (ec_pdo_list_copy(&fsm->pdos,
&fsm->slave->config->sync_configs[fsm->sync_index].pdos)) {
fsm->state = ec_fsm_pdo_state_error;
return;
}
if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {
if (!list_empty(&fsm->pdos.list))
EC_WARN("Pdos configured for SM%u, but slave %u does not "
"provide a sync manager configuration!\n",
fsm->sync_index, fsm->slave->ring_position);
continue;
}
// get first configured Pdo
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
// no pdos configured
ec_fsm_pdo_conf_action_check_assignment(fsm);
return;
}
ec_fsm_pdo_conf_action_pdo_mapping(fsm);
return;
}
fsm->state = ec_fsm_pdo_state_end;
}
/*****************************************************************************/
/**
*/
void ec_fsm_pdo_conf_action_pdo_mapping(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
const ec_pdo_t *assigned_pdo;
fsm->slave_pdo.index = fsm->pdo->index;
if ((assigned_pdo = ec_slave_find_pdo(fsm->slave, fsm->pdo->index))) {
ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);
} else { // configured Pdo is not assigned and thus unknown
ec_pdo_clear_entries(&fsm->slave_pdo);
}
if (list_empty(&fsm->slave_pdo.entries)) {
if (fsm->slave->master->debug_level)
EC_DBG("Reading mapping of Pdo 0x%04X.\n",
fsm->pdo->index);
// pdo mapping is unknown; start loading it
ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
&fsm->slave_pdo);
fsm->state = ec_fsm_pdo_conf_state_read_mapping;
fsm->state(fsm); // execute immediately
return;
}
// pdo mapping is known, check if it most be re-configured
ec_fsm_pdo_conf_action_check_mapping(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_pdo_conf_state_read_mapping(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
return;
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
EC_WARN("Failed to read mapped Pdo entries for Pdo 0x%04X.\n",
fsm->pdo->index);
// check if the mapping must be re-configured
ec_fsm_pdo_conf_action_check_mapping(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_pdo_conf_action_check_mapping(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
if (fsm->slave->master->debug_level)
EC_DBG("Mapping of Pdo 0x%04X is already configured correctly.\n",
fsm->pdo->index);
ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
return;
}
if (fsm->slave->master->debug_level) {
// TODO display diff
EC_DBG("Changing mapping of Pdo 0x%04X.\n", fsm->pdo->index);
}
ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
fsm->pdo);
fsm->state = ec_fsm_pdo_conf_state_mapping;
fsm->state(fsm); // execure immediately
}
/*****************************************************************************/
/**
*/
void ec_fsm_pdo_conf_state_mapping(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
return;
if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
EC_WARN("Failed to configure mapping of Pdo 0x%04X.\n",
fsm->pdo->index);
ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_pdo_conf_action_next_pdo_mapping(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
// get next configured Pdo
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
// no more configured pdos
ec_fsm_pdo_conf_action_check_assignment(fsm);
return;
}
ec_fsm_pdo_conf_action_pdo_mapping(fsm);
}
/*****************************************************************************/
/**
*/
void ec_fsm_pdo_conf_action_check_assignment(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
// check if assignment has to be re-configured
if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
if (fsm->slave->master->debug_level)
EC_DBG("Pdo assignment for SM%u is already configured "
"correctly.\n", fsm->sync_index);
ec_fsm_pdo_conf_action_next_sync(fsm);
return;
}
if (fsm->slave->master->debug_level) {
EC_DBG("Pdo assignment of SM%u differs:\n", fsm->sync_index);
EC_DBG("Currently assigned Pdos: ");
ec_pdo_list_print(&fsm->sync->pdos);
printk("\n");
EC_DBG("Pdos to assign: ");
ec_pdo_list_print(&fsm->pdos);
printk("\n");
}
// Pdo assignment has to be changed. Does the slave support this?
if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
|| (fsm->slave->sii.has_general
&& !fsm->slave->sii.coe_details.enable_pdo_assign)) {
EC_WARN("Slave %u does not support assigning Pdos!\n",
fsm->slave->ring_position);
ec_fsm_pdo_conf_action_next_sync(fsm);
return;
}
if (ec_sdo_request_alloc(&fsm->request, 2)) {
fsm->state = ec_fsm_pdo_state_error;
return;
}
// set mapped Pdo count to zero
EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting number of assigned Pdos to zero.\n");
fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Set the number of assigned Pdos to zero.
*/
void ec_fsm_pdo_conf_state_zero_pdo_count(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe))
return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to clear Pdo assignment of SM%u.\n", fsm->sync_index);
fsm->state = ec_fsm_pdo_state_error;
return;
}
// the sync manager's assigned Pdos have been cleared
ec_pdo_list_clear_pdos(&fsm->sync->pdos);
// assign all Pdos belonging to the current sync manager
// find first Pdo
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
if (fsm->slave->master->debug_level)
EC_DBG("No Pdos to assign.\n");
// check for mapping to be altered
ec_fsm_pdo_conf_action_next_sync(fsm);
return;
}
// assign first Pdo
fsm->pdo_pos = 1;
ec_fsm_pdo_conf_action_assign_pdo(fsm);
}
/*****************************************************************************/
/** Assign a Pdo.
*/
void ec_fsm_pdo_conf_action_assign_pdo(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
EC_WRITE_U16(fsm->request.data, fsm->pdo->index);
fsm->request.data_size = 2;
ec_sdo_request_address(&fsm->request,
0x1C10 + fsm->sync_index, fsm->pdo_pos);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Assigning Pdo 0x%04X at position %u.\n",
fsm->pdo->index, fsm->pdo_pos);
fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Add a Pdo to the sync managers Pdo assignment.
*/
void ec_fsm_pdo_conf_state_assign_pdo(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to assign Pdo 0x%04X at position %u of SM%u.\n",
fsm->pdo->index, fsm->pdo_pos, fsm->sync_index);
fsm->state = ec_fsm_pdo_state_error;
return;
}
// find next Pdo
if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
// no more Pdos to assign, set Pdo count
EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting number of assigned Pdos to %u.\n", fsm->pdo_pos);
fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// add next Pdo to assignment
fsm->pdo_pos++;
ec_fsm_pdo_conf_action_assign_pdo(fsm);
}
/*****************************************************************************/
/** Set the number of assigned Pdos.
*/
void ec_fsm_pdo_conf_state_set_pdo_count(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to set number of assigned Pdos of SM%u.\n",
fsm->sync_index);
fsm->state = ec_fsm_pdo_state_error;
return;
}
// Pdos have been configured
ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);
if (fsm->slave->master->debug_level)
EC_DBG("Successfully configured Pdo assignment of SM%u.\n",
fsm->sync_index);
// check if Pdo mapping has to be altered
ec_fsm_pdo_conf_action_next_sync(fsm);
}
/******************************************************************************
* Common state functions
*****************************************************************************/
/** State: ERROR.
*/
void ec_fsm_pdo_state_error(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
}
/*****************************************************************************/
/** State: END.
*/
void ec_fsm_pdo_state_end(
ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */
)
{
}
/*****************************************************************************/

47
master/fsm_pdo_assign.h → master/fsm_pdo.h
View File

@ -33,54 +33,57 @@
/**
\file
EtherCAT Pdo assignment state machine structures.
EtherCAT Pdo configuration state machine structures.
*/
/*****************************************************************************/
#ifndef __EC_FSM_PDO_ASSIGN_H__
#define __EC_FSM_PDO_ASSIGN_H__
#ifndef __EC_FSM_PDO_H__
#define __EC_FSM_PDO_H__
#include "../include/ecrt.h"
#include "globals.h"
#include "datagram.h"
#include "fsm_coe.h"
#include "fsm_pdo_entry.h"
/*****************************************************************************/
/**
* \see ec_fsm_pdo_assign
* \see ec_fsm_pdo
*/
typedef struct ec_fsm_pdo_assign ec_fsm_pdo_assign_t;
typedef struct ec_fsm_pdo ec_fsm_pdo_t;
/** Pdo assignment state machine.
/** Pdo configuration state machine.
*/
struct ec_fsm_pdo_assign
struct ec_fsm_pdo
{
void (*state)(ec_fsm_pdo_assign_t *); /**< State function. */
void (*state)(ec_fsm_pdo_t *); /**< State function. */
ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */
ec_slave_t *slave; /**< Slave the FSM runs on. */
uint8_t sync_index; /**< Current sync manager index. */
const ec_pdo_list_t *pdos; /**< Target Pdo assignment. */
const ec_sync_t *sync; /**< Current sync manager. */
unsigned int num_configured_syncs; /**< Number of configured
assignments. */
const ec_pdo_t *pdo; /**< Current Pdo. */
ec_fsm_pdo_entry_t fsm_pdo_entry; /**< Pdo entry state machine. */
ec_pdo_list_t pdos; /**< Pdo configuration. */
ec_sdo_request_t request; /**< Sdo request. */
ec_pdo_t slave_pdo; /**< Pdo actually appearing in a slave. */
ec_slave_t *slave; /**< Slave the FSM runs on. */
uint8_t sync_index; /**< Current sync manager index. */
ec_sync_t *sync; /**< Current sync manager. */
ec_pdo_t *pdo; /**< Current Pdo. */
unsigned int pdo_pos; /**< Assignment position of current Pdos. */
unsigned int pdo_count; /**< Number of assigned Pdos. */
};
/*****************************************************************************/
void ec_fsm_pdo_assign_init(ec_fsm_pdo_assign_t *, ec_fsm_coe_t *);
void ec_fsm_pdo_assign_clear(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_init(ec_fsm_pdo_t *, ec_fsm_coe_t *);
void ec_fsm_pdo_clear(ec_fsm_pdo_t *);
void ec_fsm_pdo_assign_start(ec_fsm_pdo_assign_t *, ec_slave_t *);
int ec_fsm_pdo_assign_exec(ec_fsm_pdo_assign_t *);
int ec_fsm_pdo_assign_success(const ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_start_reading(ec_fsm_pdo_t *, ec_slave_t *);
void ec_fsm_pdo_start_configuration(ec_fsm_pdo_t *, ec_slave_t *);
int ec_fsm_pdo_exec(ec_fsm_pdo_t *);
int ec_fsm_pdo_success(const ec_fsm_pdo_t *);
/*****************************************************************************/

389
master/fsm_pdo_assign.c
View File

@ -1,389 +0,0 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it
* and/or modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* The IgH EtherCAT Master is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* The right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
/** \file
* EtherCAT Pdo assignment state machine.
*/
/*****************************************************************************/
#include "globals.h"
#include "master.h"
#include "mailbox.h"
#include "slave_config.h"
#include "fsm_pdo_assign.h"
/*****************************************************************************/
void ec_fsm_pdo_assign_state_start(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_assign_state_zero_count(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_assign_state_add_pdo(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_assign_state_pdo_count(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_assign_state_end(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_assign_state_error(ec_fsm_pdo_assign_t *);
void ec_fsm_pdo_assign_next_sync(ec_fsm_pdo_assign_t *);
/*****************************************************************************/
/** Constructor.
*/
void ec_fsm_pdo_assign_init(
ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */
ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */
)
{
fsm->fsm_coe = fsm_coe;
ec_sdo_request_init(&fsm->request);
}
/*****************************************************************************/
/** Destructor.
*/
void ec_fsm_pdo_assign_clear(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
ec_sdo_request_clear(&fsm->request);
}
/*****************************************************************************/
/** Start Pdo assignment state machine.
*/
void ec_fsm_pdo_assign_start(
ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */
ec_slave_t *slave /**< slave to configure */
)
{
fsm->slave = slave;
fsm->state = ec_fsm_pdo_assign_state_start;
}
/*****************************************************************************/
/** Get running state.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_assign_running(
const ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
return fsm->state != ec_fsm_pdo_assign_state_end
&& fsm->state != ec_fsm_pdo_assign_state_error;
}
/*****************************************************************************/
/** Executes the current state of the state machine.
*
* If the state machine's datagram is not sent or received yet, the execution
* of the state machine is delayed to the next cycle.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_assign_exec(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
fsm->state(fsm);
return ec_fsm_pdo_assign_running(fsm);
}
/*****************************************************************************/
/** Get execution result.
*
* \return true, if the state machine terminated gracefully
*/
int ec_fsm_pdo_assign_success(
const ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
return fsm->state == ec_fsm_pdo_assign_state_end;
}
/******************************************************************************
* State functions.
*****************************************************************************/
/** Start Pdo assignment.
*/
void ec_fsm_pdo_assign_state_start(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
if (!fsm->slave->config) {
fsm->state = ec_fsm_pdo_assign_state_end;
return;
}
fsm->sync_index = 0xff; // next is zero
fsm->num_configured_syncs = 0;
ec_fsm_pdo_assign_next_sync(fsm);
}
/*****************************************************************************/
/** Process Pdo assignment of next sync manager.
*/
void ec_fsm_pdo_assign_next_sync(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
fsm->sync_index++;
for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
fsm->pdos = &fsm->slave->config->sync_configs[fsm->sync_index].pdos;
if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {
if (!list_empty(&fsm->pdos->list)) {
EC_WARN("Slave %u does not provide a configuration "
"for SM%u!\n", fsm->slave->ring_position,
fsm->sync_index);
fsm->state = ec_fsm_pdo_assign_state_error;
return;
}
continue;
}
// check if assignment has to be altered
if (ec_pdo_list_equal(&fsm->sync->pdos, fsm->pdos))
continue;
if (fsm->slave->master->debug_level) {
EC_DBG("Pdo assignment of SM%u differs:\n", fsm->sync_index);
EC_DBG("Currently assigned Pdos: ");
ec_pdo_list_print(&fsm->sync->pdos);
printk("\n");
EC_DBG("Pdos to assign: ");
ec_pdo_list_print(fsm->pdos);
printk("\n");
}
// Pdo assignment has to be changed. Does the slave support this?
if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
|| (fsm->slave->sii.has_general
&& !fsm->slave->sii.coe_details.enable_pdo_assign)) {
EC_WARN("Slave %u does not support assigning Pdos!\n",
fsm->slave->ring_position);
fsm->state = ec_fsm_pdo_assign_state_error;
return;
}
fsm->num_configured_syncs++;
if (ec_sdo_request_alloc(&fsm->request, 2)) {
fsm->state = ec_fsm_pdo_assign_state_error;
return;
}
// set mapped Pdo count to zero
EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting Pdo count to zero.\n");
fsm->state = ec_fsm_pdo_assign_state_zero_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
if (fsm->slave->master->debug_level && !fsm->num_configured_syncs)
EC_DBG("Pdo assignments are already configured correctly.\n");
fsm->state = ec_fsm_pdo_assign_state_end;
}
/*****************************************************************************/
/** Assign next Pdo.
*/
ec_pdo_t *ec_fsm_pdo_assign_next_pdo(
const ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */
const struct list_head *list /**< current Pdo list item */
)
{
list = list->next;
if (list == &fsm->pdos->list)
return NULL; // no next Pdo
return list_entry(list, ec_pdo_t, list);
}
/*****************************************************************************/
/** Assign a Pdo.
*/
void ec_fsm_pdo_assign_add_pdo(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
EC_WRITE_U16(fsm->request.data, fsm->pdo->index);
fsm->request.data_size = 2;
ec_sdo_request_address(&fsm->request,
0x1C10 + fsm->sync_index, fsm->pdo_count);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Assigning Pdo 0x%04X at position %u.\n",
fsm->pdo->index, fsm->pdo_count);
fsm->state = ec_fsm_pdo_assign_state_add_pdo;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Set the number of assigned Pdos to zero.
*/
void ec_fsm_pdo_assign_state_zero_count(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to clear Pdo assignment of SM%u.\n", fsm->sync_index);
fsm->state = ec_fsm_pdo_assign_state_error;
return;
}
// assign all Pdos belonging to the current sync manager
// find first Pdo
if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdos->list))) {
if (fsm->slave->master->debug_level)
EC_DBG("No Pdos to assign.\n");
ec_fsm_pdo_assign_next_sync(fsm);
return;
}
// assign first Pdo
fsm->pdo_count = 1;
ec_fsm_pdo_assign_add_pdo(fsm);
}
/*****************************************************************************/
/** Add a Pdo to the sync managers Pdo assignment.
*/
void ec_fsm_pdo_assign_state_add_pdo(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to assign Pdo 0x%04X to position %u in SM%u.\n",
fsm->pdo->index, fsm->pdo_count, fsm->sync_index);
fsm->state = ec_fsm_pdo_assign_state_error;
return;
}
// find next Pdo
if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdo->list))) {
// no more Pdos to map. write Pdo count
EC_WRITE_U8(fsm->request.data, fsm->pdo_count);
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting number of assigned Pdos to %u.\n",
fsm->pdo_count);
fsm->state = ec_fsm_pdo_assign_state_pdo_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// add next Pdo to assignment
fsm->pdo_count++;
ec_fsm_pdo_assign_add_pdo(fsm);
}
/*****************************************************************************/
/** Set the number of assigned Pdos.
*/
void ec_fsm_pdo_assign_state_pdo_count(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to set number of assigned Pdos for SM%u.\n",
fsm->sync_index);
fsm->state = ec_fsm_pdo_assign_state_error;
return;
}
if (fsm->slave->master->debug_level)
EC_DBG("Pdo assignment successful for SM%u.\n", fsm->sync_index);
// assignment for this sync manager finished
ec_fsm_pdo_assign_next_sync(fsm);
}
/******************************************************************************
* Common state functions
*****************************************************************************/
/** State: ERROR.
*/
void ec_fsm_pdo_assign_state_error(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
}
/*****************************************************************************/
/** State: END.
*/
void ec_fsm_pdo_assign_state_end(
ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */
)
{
}
/*****************************************************************************/

500
master/fsm_pdo_entry.c Normal file
View File

@ -0,0 +1,500 @@
/******************************************************************************
*
* $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 mapping state machine.
*/
/*****************************************************************************/
#include "globals.h"
#include "master.h"
#include "mailbox.h"
#include "slave_config.h"
#include "fsm_pdo_entry.h"
/*****************************************************************************/
void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);
/*****************************************************************************/
/** Constructor.
*/
void ec_fsm_pdo_entry_init(
ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */
ec_fsm_coe_t *fsm_coe /**< CoE state machine to use. */
)
{
fsm->fsm_coe = fsm_coe;
ec_sdo_request_init(&fsm->request);
}
/*****************************************************************************/
/** Destructor.
*/
void ec_fsm_pdo_entry_clear(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
ec_sdo_request_clear(&fsm->request);
}
/*****************************************************************************/
/** Start reading a Pdo's entries.
*/
void ec_fsm_pdo_entry_start_reading(
ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */
ec_slave_t *slave, /**< slave to configure */
ec_pdo_t *pdo /**< Pdo to read entries for. */
)
{
fsm->slave = slave;
fsm->target_pdo = pdo;
ec_pdo_clear_entries(fsm->target_pdo);
fsm->state = ec_fsm_pdo_entry_read_state_start;
}
/*****************************************************************************/
/** Start Pdo mapping state machine.
*/
void ec_fsm_pdo_entry_start_configuration(
ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */
ec_slave_t *slave, /**< slave to configure */
const ec_pdo_t *pdo /**< Pdo with the desired entries. */
)
{
fsm->slave = slave;
fsm->source_pdo = pdo;
fsm->state = ec_fsm_pdo_entry_conf_state_start;
}
/*****************************************************************************/
/** Get running state.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_entry_running(
const ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
return fsm->state != ec_fsm_pdo_entry_state_end
&& fsm->state != ec_fsm_pdo_entry_state_error;
}
/*****************************************************************************/
/** Executes the current state.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_entry_exec(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
fsm->state(fsm);
return ec_fsm_pdo_entry_running(fsm);
}
/*****************************************************************************/
/** Get execution result.
*
* \return true, if the state machine terminated gracefully
*/
int ec_fsm_pdo_entry_success(
const ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
return fsm->state == ec_fsm_pdo_entry_state_end;
}
/******************************************************************************
* Reading state functions.
*****************************************************************************/
/** Request reading the number of mapped Pdo entries.
*/
void ec_fsm_pdo_entry_read_state_start(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, 0);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_entry_read_state_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Read number of mapped Pdo entries.
*/
void ec_fsm_pdo_entry_read_state_count(
ec_fsm_pdo_entry_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.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint8_t)) {
EC_ERR("Invalid data size %u at uploading Sdo 0x%04X:%02X.\n",
fsm->request.data_size, fsm->request.index,
fsm->request.subindex);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
fsm->entry_count = EC_READ_U8(fsm->request.data);
if (fsm->slave->master->debug_level)
EC_DBG("%u Pdo entries mapped.\n", fsm->entry_count);
// read first Pdo entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_read_action_next(fsm);
}
/*****************************************************************************/
/** Read next Pdo entry.
*/
void ec_fsm_pdo_entry_read_action_next(
ec_fsm_pdo_entry_t *fsm /**< finite state machine */
)
{
if (fsm->entry_pos <= fsm->entry_count) {
ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, fsm->entry_pos);
ecrt_sdo_request_read(&fsm->request);
fsm->state = ec_fsm_pdo_entry_read_state_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// finished reading entries.
fsm->state = ec_fsm_pdo_entry_state_end;
}
/*****************************************************************************/
/** Read Pdo entry information.
*/
void ec_fsm_pdo_entry_read_state_entry(
ec_fsm_pdo_entry_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 entry.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint32_t)) {
EC_ERR("Invalid data size %u at uploading Sdo 0x%04X:%02X.\n",
fsm->request.data_size, fsm->request.index,
fsm->request.subindex);
fsm->state = ec_fsm_pdo_entry_state_error;
} else {
uint32_t pdo_entry_info;
ec_pdo_entry_t *pdo_entry;
pdo_entry_info = EC_READ_U32(fsm->request.data);
if (!(pdo_entry = (ec_pdo_entry_t *)
kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate Pdo entry.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
ec_pdo_entry_init(pdo_entry);
pdo_entry->index = pdo_entry_info >> 16;
pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;
pdo_entry->bit_length = pdo_entry_info & 0xFF;
if (!pdo_entry->index && !pdo_entry->subindex) {
if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {
ec_pdo_entry_clear(pdo_entry);
kfree(pdo_entry);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
}
if (fsm->slave->master->debug_level) {
EC_DBG("Pdo entry 0x%04X:%02X, %u bit, \"%s\".\n",
pdo_entry->index, pdo_entry->subindex,
pdo_entry->bit_length,
pdo_entry->name ? pdo_entry->name : "???");
}
list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries);
// next Pdo entry
fsm->entry_pos++;
ec_fsm_pdo_entry_read_action_next(fsm);
}
}
/******************************************************************************
* Configuration state functions.
*****************************************************************************/
/** Start Pdo mapping.
*/
void ec_fsm_pdo_entry_conf_state_start(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
// Pdo mapping has to be changed. Does the slave support this?
if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
|| (fsm->slave->sii.has_general
&& !fsm->slave->sii.coe_details.enable_pdo_configuration)) {
EC_WARN("Slave %u does not support changing the Pdo mapping!\n",
fsm->slave->ring_position);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (ec_sdo_request_alloc(&fsm->request, 4)) {
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// set mapped Pdo entry count to zero
EC_WRITE_U8(fsm->request.data, 0);
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting entry count to zero.\n");
fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Process next Pdo entry.
*/
ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(
const ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */
const struct list_head *list /**< current entry list item */
)
{
list = list->next;
if (list == &fsm->source_pdo->entries)
return NULL; // no next entry
return list_entry(list, ec_pdo_entry_t, list);
}
/*****************************************************************************/
/** Set the number of mapped entries to zero.
*/
void ec_fsm_pdo_entry_conf_state_zero_entry_count(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe))
return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to clear Pdo mapping.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// find first entry
if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
fsm, &fsm->source_pdo->entries))) {
if (fsm->slave->master->debug_level)
EC_DBG("No entries to map.\n");
fsm->state = ec_fsm_pdo_entry_state_end; // finished
return;
}
// add first entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_conf_action_map(fsm);
}
/*****************************************************************************/
/** Starts to add a Pdo entry.
*/
void ec_fsm_pdo_entry_conf_action_map(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
uint32_t value;
if (fsm->slave->master->debug_level)
EC_DBG("Mapping Pdo entry 0x%04X:%02X (%u bit) at position %u.\n",
fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_pos);
value = fsm->entry->index << 16
| fsm->entry->subindex << 8 | fsm->entry->bit_length;
EC_WRITE_U32(fsm->request.data, value);
fsm->request.data_size = 4;
ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, fsm->entry_pos);
ecrt_sdo_request_write(&fsm->request);
fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Add a Pdo entry.
*/
void ec_fsm_pdo_entry_conf_state_map_entry(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to map Pdo entry 0x%04X:%02X (%u bit) to "
"position %u.\n", fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_pos);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// find next entry
if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
fsm, &fsm->entry->list))) {
// No more entries to add. Write entry count.
EC_WRITE_U8(fsm->request.data, fsm->entry_pos);
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_pos);
fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// add next entry
fsm->entry_pos++;
ec_fsm_pdo_entry_conf_action_map(fsm);
}
/*****************************************************************************/
/** Set the number of entries.
*/
void ec_fsm_pdo_entry_conf_state_set_entry_count(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping 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.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (fsm->slave->master->debug_level)
EC_DBG("Successfully configured mapping for Pdo 0x%04X.\n",
fsm->source_pdo->index);
fsm->state = ec_fsm_pdo_entry_state_end; // finished
}
/******************************************************************************
* Common state functions
*****************************************************************************/
/** State: ERROR.
*/
void ec_fsm_pdo_entry_state_error(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
}
/*****************************************************************************/
/** State: END.
*/
void ec_fsm_pdo_entry_state_end(
ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */
)
{
}
/*****************************************************************************/

42
master/fsm_pdo_mapping.h → master/fsm_pdo_entry.h
View File

@ -32,13 +32,13 @@
*****************************************************************************/
/** \file
* EtherCAT Pdo configuration state machine structures.
* EtherCAT Pdo entry configuration state machine structures.
*/
/*****************************************************************************/
#ifndef __EC_FSM_PDO_MAPPING_H__
#define __EC_FSM_PDO_MAPPING_H__
#ifndef __EC_FSM_PDO_ENTRY_H__
#define __EC_FSM_PDO_ENTRY_H__
#include "globals.h"
#include "../include/ecrt.h"
@ -48,34 +48,38 @@
/*****************************************************************************/
/**
* \see ec_fsm_pdo_mapping
* \see ec_fsm_pdo_entry
*/
typedef struct ec_fsm_pdo_mapping ec_fsm_pdo_mapping_t;
typedef struct ec_fsm_pdo_entry ec_fsm_pdo_entry_t;
/** Pdo configuration state machine.
*/
struct ec_fsm_pdo_mapping
struct ec_fsm_pdo_entry
{
void (*state)(ec_fsm_pdo_mapping_t *); /**< state function */
void (*state)(ec_fsm_pdo_entry_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. */
unsigned int num_configured_pdos; /**< Number of configured Pdos. */
const ec_pdo_entry_t *entry; /**< Current entry. */
ec_sdo_request_t request; /**< Sdo request. */
unsigned int entry_count; /**< Number of configured entries. */
ec_slave_t *slave; /**< Slave the FSM runs on. */
ec_pdo_t *target_pdo; /**< Pdo to read the mapping for. */
const ec_pdo_t *source_pdo; /**< Pdo with desired mapping. */
const ec_pdo_entry_t *entry; /**< Current entry. */
unsigned int entry_count; /**< Number of entries. */
unsigned int entry_pos; /**< Position in Pdo mapping. */
};
/*****************************************************************************/
void ec_fsm_pdo_mapping_init(ec_fsm_pdo_mapping_t *, ec_fsm_coe_t *);
void ec_fsm_pdo_mapping_clear(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_entry_init(ec_fsm_pdo_entry_t *, ec_fsm_coe_t *);
void ec_fsm_pdo_entry_clear(ec_fsm_pdo_entry_t *);
void ec_fsm_pdo_mapping_start(ec_fsm_pdo_mapping_t *, ec_slave_t *);
int ec_fsm_pdo_mapping_exec(ec_fsm_pdo_mapping_t *);
int ec_fsm_pdo_mapping_success(const ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_entry_start_reading(ec_fsm_pdo_entry_t *, ec_slave_t *,
ec_pdo_t *);
void ec_fsm_pdo_entry_start_configuration(ec_fsm_pdo_entry_t *, ec_slave_t *,
const ec_pdo_t *);
int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *);
int ec_fsm_pdo_entry_success(const ec_fsm_pdo_entry_t *);
/*****************************************************************************/

383
master/fsm_pdo_mapping.c
View File

@ -1,383 +0,0 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it
* and/or modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* The IgH EtherCAT Master is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* The right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
/** \file
* EtherCAT Pdo mapping state machine.
*/
/*****************************************************************************/
#include "globals.h"
#include "master.h"
#include "mailbox.h"
#include "slave_config.h"
#include "fsm_pdo_mapping.h"
/*****************************************************************************/
void ec_fsm_pdo_mapping_state_start(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_mapping_state_zero_count(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_mapping_state_add_entry(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_mapping_state_entry_count(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_mapping_state_end(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_mapping_state_error(ec_fsm_pdo_mapping_t *);
void ec_fsm_pdo_mapping_next_pdo(ec_fsm_pdo_mapping_t *);
/*****************************************************************************/
/** Constructor.
*/
void ec_fsm_pdo_mapping_init(
ec_fsm_pdo_mapping_t *fsm, /**< Pdo mapping state machine. */
ec_fsm_coe_t *fsm_coe /**< CoE state machine to use. */
)
{
fsm->fsm_coe = fsm_coe;
ec_sdo_request_init(&fsm->request);
}
/*****************************************************************************/
/** Destructor.
*/
void ec_fsm_pdo_mapping_clear(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
ec_sdo_request_clear(&fsm->request);
}
/*****************************************************************************/
/** Start Pdo mapping state machine.
*/
void ec_fsm_pdo_mapping_start(
ec_fsm_pdo_mapping_t *fsm, /**< Pdo mapping state machine. */
ec_slave_t *slave /**< slave to configure */
)
{
fsm->slave = slave;
fsm->state = ec_fsm_pdo_mapping_state_start;
}
/*****************************************************************************/
/** Get running state.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_mapping_running(
const ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
return fsm->state != ec_fsm_pdo_mapping_state_end
&& fsm->state != ec_fsm_pdo_mapping_state_error;
}
/*****************************************************************************/
/** Executes the current state.
*
* \return false, if state machine has terminated
*/
int ec_fsm_pdo_mapping_exec(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
fsm->state(fsm);
return ec_fsm_pdo_mapping_running(fsm);
}
/*****************************************************************************/
/** Get execution result.
*
* \return true, if the state machine terminated gracefully
*/
int ec_fsm_pdo_mapping_success(
const ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
return fsm->state == ec_fsm_pdo_mapping_state_end;
}
/******************************************************************************
* State functions.
*****************************************************************************/
/** Start Pdo mapping.
*/
void ec_fsm_pdo_mapping_state_start(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
if (!fsm->slave->config) {
fsm->state = ec_fsm_pdo_mapping_state_end;
return;
}
fsm->pdo = NULL;
fsm->num_configured_pdos = 0;
ec_fsm_pdo_mapping_next_pdo(fsm);
}
/*****************************************************************************/
/** Process mapping of next Pdo.
*/
void ec_fsm_pdo_mapping_next_pdo(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
uint8_t sync_index;
const ec_pdo_list_t *pdos;
const ec_pdo_t *pdo, *assigned_pdo;
for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
pdos = &fsm->slave->config->sync_configs[sync_index].pdos;
list_for_each_entry(pdo, &pdos->list, list) {
if (fsm->pdo) { // there was a Pdo mapping changed in the last run
if (pdo == fsm->pdo) // this is the previously configured Pdo
fsm->pdo = NULL; // take the next one
} else {
if ((assigned_pdo = ec_slave_find_pdo(fsm->slave, pdo->index)))
if (ec_pdo_equal_entries(pdo, assigned_pdo))
continue; // Pdo entries mapped correctly
fsm->pdo = pdo;
fsm->num_configured_pdos++;
break;
}
}
}
if (!fsm->pdo) {
if (fsm->slave->master->debug_level && !fsm->num_configured_pdos)
EC_DBG("Pdo mappings are already configured correctly.\n");
fsm->state = ec_fsm_pdo_mapping_state_end;
return;
}
// Pdo mapping has to be changed. Does the slave support this?
if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
|| (fsm->slave->sii.has_general
&& !fsm->slave->sii.coe_details.enable_pdo_configuration)) {
EC_WARN("Slave %u does not support Pdo mapping configuration!\n",
fsm->slave->ring_position);
fsm->state = ec_fsm_pdo_mapping_state_error;
return;
}
if (fsm->slave->master->debug_level) {
EC_DBG("Changing mapping of Pdo 0x%04X.\n", fsm->pdo->index);
}
if (ec_sdo_request_alloc(&fsm->request, 4)) {
fsm->state = ec_fsm_pdo_mapping_state_error;
return;
}
// set mapped Pdo count to zero
EC_WRITE_U8(fsm->request.data, 0);
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting entry count to zero.\n");
fsm->state = ec_fsm_pdo_mapping_state_zero_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Process next Pdo entry.
*/
ec_pdo_entry_t *ec_fsm_pdo_mapping_next_entry(
const ec_fsm_pdo_mapping_t *fsm, /**< Pdo mapping 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_mapping_add_entry(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
uint32_t value;
if (fsm->slave->master->debug_level)
EC_DBG("Mapping Pdo entry 0x%04X:%02X (%u bit) at position %u.\n",
fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_count);
value = fsm->entry->index << 16
| fsm->entry->subindex << 8 | fsm->entry->bit_length;
EC_WRITE_U32(fsm->request.data, value);
fsm->request.data_size = 4;
ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count);
ecrt_sdo_request_write(&fsm->request);
fsm->state = ec_fsm_pdo_mapping_state_add_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
/** Set the number of mapped entries to zero.
*/
void ec_fsm_pdo_mapping_state_zero_count(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to clear mapping of Pdo 0x%04X.\n", fsm->pdo->index);
fsm->state = ec_fsm_pdo_mapping_state_error;
return;
}
// find first entry
if (!(fsm->entry =
ec_fsm_pdo_mapping_next_entry(fsm, &fsm->pdo->entries))) {
if (fsm->slave->master->debug_level)
EC_DBG("No entries to map.\n");
ec_fsm_pdo_mapping_next_pdo(fsm);
return;
}
// add first entry
fsm->entry_count = 1;
ec_fsm_pdo_mapping_add_entry(fsm);
}
/*****************************************************************************/
/** Add a Pdo entry.
*/
void ec_fsm_pdo_mapping_state_add_entry(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to map Pdo entry 0x%04X:%02X at 0x%04X:%02X.\n",
fsm->entry->index, fsm->entry->subindex, fsm->pdo->index,
fsm->entry_count);
fsm->state = ec_fsm_pdo_mapping_state_error;
return;
}
// find next entry
if (!(fsm->entry = ec_fsm_pdo_mapping_next_entry(fsm, &fsm->entry->list))) {
// No more entries to add. Write entry count.
EC_WRITE_U8(fsm->request.data, fsm->entry_count);
fsm->request.data_size = 1;
ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
if (fsm->slave->master->debug_level)
EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_count);
fsm->state = ec_fsm_pdo_mapping_state_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
// add next entry
fsm->entry_count++;
ec_fsm_pdo_mapping_add_entry(fsm);
}
/*****************************************************************************/
/** Set the number of entries.
*/
void ec_fsm_pdo_mapping_state_entry_count(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_WARN("Failed to set number of entries for Pdo 0x%04X.\n",
fsm->pdo->index);
fsm->state = ec_fsm_pdo_mapping_state_error;
return;
}
if (fsm->slave->master->debug_level)
EC_DBG("Pdo mapping configuration successful.\n");
ec_fsm_pdo_mapping_next_pdo(fsm);
}
/******************************************************************************
* Common state functions
*****************************************************************************/
/** State: ERROR.
*/
void ec_fsm_pdo_mapping_state_error(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
}
/*****************************************************************************/
/** State: END.
*/
void ec_fsm_pdo_mapping_state_end(
ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */
)
{
}
/*****************************************************************************/

112
master/fsm_slave_config.c
View File

@ -53,8 +53,7 @@ void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_pdo_assign(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_pdo_mapping(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);
@ -75,16 +74,16 @@ void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);
*/
void ec_fsm_slave_config_init(
ec_fsm_slave_config_t *fsm, /**< slave state machine */
ec_datagram_t *datagram /**< datagram structure to use */
ec_datagram_t *datagram, /**< datagram structure to use */
ec_fsm_change_t *fsm_change, /**< State change state machine to use. */
ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */
ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration state machine to use. */
)
{
fsm->datagram = datagram;
// init sub state machines
ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
ec_fsm_pdo_assign_init(&fsm->fsm_pdo_assign, &fsm->fsm_coe);
ec_fsm_pdo_mapping_init(&fsm->fsm_pdo_mapping, &fsm->fsm_coe);
fsm->fsm_change = fsm_change;
fsm->fsm_coe = fsm_coe;
fsm->fsm_pdo = fsm_pdo;
}
/*****************************************************************************/
@ -95,11 +94,6 @@ void ec_fsm_slave_config_clear(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
// clear sub state machines
ec_fsm_change_clear(&fsm->fsm_change);
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_fsm_pdo_assign_clear(&fsm->fsm_pdo_assign);
ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_mapping);
}
/*****************************************************************************/
@ -181,8 +175,8 @@ void ec_fsm_slave_config_state_start(
// force flag
fsm->slave->force_config = 0;
ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);
ec_fsm_change_exec(&fsm->fsm_change);
ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);
ec_fsm_change_exec(fsm->fsm_change);
fsm->state = ec_fsm_slave_config_state_init;
}
@ -198,10 +192,10 @@ void ec_fsm_slave_config_state_init(
ec_slave_t *slave = fsm->slave;
ec_datagram_t *datagram = fsm->datagram;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (ec_fsm_change_exec(fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
if (!fsm->fsm_change.spontaneous_change)
if (!ec_fsm_change_success(fsm->fsm_change)) {
if (!fsm->fsm_change->spontaneous_change)
slave->error_flag = 1;
fsm->state = ec_fsm_slave_config_state_error;
return;
@ -385,8 +379,8 @@ void ec_fsm_slave_config_enter_preop(
)
{
fsm->state = ec_fsm_slave_config_state_preop;
ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP);
ec_fsm_change_exec(&fsm->fsm_change); // execute immediately
ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP);
ec_fsm_change_exec(fsm->fsm_change); // execute immediately
}
/*****************************************************************************/
@ -400,10 +394,10 @@ void ec_fsm_slave_config_state_preop(
ec_slave_t *slave = fsm->slave;
ec_master_t *master = fsm->slave->master;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (ec_fsm_change_exec(fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
if (!fsm->fsm_change.spontaneous_change)
if (!ec_fsm_change_success(fsm->fsm_change)) {
if (!fsm->fsm_change->spontaneous_change)
slave->error_flag = 1;
fsm->state = ec_fsm_slave_config_state_error;
return;
@ -455,8 +449,8 @@ void ec_fsm_slave_config_enter_sdo_conf(
fsm->request = list_entry(fsm->slave->config->sdo_configs.next,
ec_sdo_request_t, list);
ecrt_sdo_request_write(fsm->request);
ec_fsm_coe_transfer(&fsm->fsm_coe, fsm->slave, fsm->request);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
}
/*****************************************************************************/
@ -467,9 +461,9 @@ void ec_fsm_slave_config_state_sdo_conf(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_ERR("Sdo configuration failed for slave %u.\n",
fsm->slave->ring_position);
fsm->slave->error_flag = 1;
@ -482,8 +476,8 @@ void ec_fsm_slave_config_state_sdo_conf(
fsm->request = list_entry(fsm->request->list.next, ec_sdo_request_t,
list);
ecrt_sdo_request_write(fsm->request);
ec_fsm_coe_transfer(&fsm->fsm_coe, fsm->slave, fsm->request);
ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
return;
}
@ -571,45 +565,25 @@ void ec_fsm_slave_config_state_pdo_sync(
return;
}
// Start configuring Pdo mapping
ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_mapping, fsm->slave);
fsm->state = ec_fsm_slave_config_state_pdo_mapping;
// Start configuring Pdos
ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave);
fsm->state = ec_fsm_slave_config_state_pdo_conf;
fsm->state(fsm); // execute immediately
}
/*****************************************************************************/
/** Slave configuration state: PDO_MAPPING.
/** Slave configuration state: PDO_CONF.
*/
void ec_fsm_slave_config_state_pdo_mapping(
void ec_fsm_slave_config_state_pdo_conf(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_mapping)) return;
if (ec_fsm_pdo_exec(fsm->fsm_pdo))
return;
if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_mapping)) {
EC_WARN("Configuration of Pdo mappings failed on slave %u.\n",
fsm->slave->ring_position);
}
// start applying Pdo assignments
ec_fsm_pdo_assign_start(&fsm->fsm_pdo_assign, fsm->slave);
fsm->state = ec_fsm_slave_config_state_pdo_assign;
fsm->state(fsm); // execute immediately
}
/*****************************************************************************/
/** Slave configuration state: PDO_ASSIGN.
*/
void ec_fsm_slave_config_state_pdo_assign(
ec_fsm_slave_config_t *fsm /**< slave state machine */
)
{
if (ec_fsm_pdo_assign_exec(&fsm->fsm_pdo_assign)) return;
if (!ec_fsm_pdo_assign_success(&fsm->fsm_pdo_assign)) {
EC_WARN("Configuration of Pdo assignments failed on slave %u.\n",
if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
EC_WARN("Pdo configuration failed on slave %u.\n",
fsm->slave->ring_position);
}
@ -708,8 +682,8 @@ void ec_fsm_slave_config_enter_safeop(
)
{
fsm->state = ec_fsm_slave_config_state_safeop;
ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP);
ec_fsm_change_exec(&fsm->fsm_change); // execute immediately
ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP);
ec_fsm_change_exec(fsm->fsm_change); // execute immediately
}
/*****************************************************************************/
@ -723,10 +697,10 @@ void ec_fsm_slave_config_state_safeop(
ec_master_t *master = fsm->slave->master;
ec_slave_t *slave = fsm->slave;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (ec_fsm_change_exec(fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
if (!fsm->fsm_change.spontaneous_change)
if (!ec_fsm_change_success(fsm->fsm_change)) {
if (!fsm->fsm_change->spontaneous_change)
fsm->slave->error_flag = 1;
fsm->state = ec_fsm_slave_config_state_error;
return;
@ -749,8 +723,8 @@ void ec_fsm_slave_config_state_safeop(
// set state to OP
fsm->state = ec_fsm_slave_config_state_op;
ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP);
ec_fsm_change_exec(&fsm->fsm_change); // execute immediately
ec_fsm_change_start(fsm->fsm_change, slave, EC_SLAVE_STATE_OP);
ec_fsm_change_exec(fsm->fsm_change); // execute immediately
}
/*****************************************************************************/
@ -764,10 +738,10 @@ void ec_fsm_slave_config_state_op(
ec_master_t *master = fsm->slave->master;
ec_slave_t *slave = fsm->slave;
if (ec_fsm_change_exec(&fsm->fsm_change)) return;
if (ec_fsm_change_exec(fsm->fsm_change)) return;
if (!ec_fsm_change_success(&fsm->fsm_change)) {
if (!fsm->fsm_change.spontaneous_change)
if (!ec_fsm_change_success(fsm->fsm_change)) {
if (!fsm->fsm_change->spontaneous_change)
slave->error_flag = 1;
fsm->state = ec_fsm_slave_config_state_error;
return;

17
master/fsm_slave_config.h
View File

@ -48,8 +48,7 @@
#include "datagram.h"
#include "fsm_change.h"
#include "fsm_coe.h"
#include "fsm_pdo_assign.h"
#include "fsm_pdo_mapping.h"
#include "fsm_pdo.h"
/*****************************************************************************/
@ -60,21 +59,21 @@ typedef struct ec_fsm_slave_config ec_fsm_slave_config_t;
*/
struct ec_fsm_slave_config
{
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_fsm_change_t *fsm_change; /**< State change state machine. */
ec_fsm_coe_t *fsm_coe; /**< CoE state machine. */
ec_fsm_pdo_t *fsm_pdo; /**< Pdo configuration state machine. */
ec_slave_t *slave; /**< Slave the FSM runs on. */
void (*state)(ec_fsm_slave_config_t *); /**< State function. */
unsigned int retries; /**< Retries on datagram timeout. */
ec_sdo_request_t *request; /**< Sdo request for Sdo configuration. */
ec_fsm_change_t fsm_change; /**< State change state machine. */
ec_fsm_coe_t fsm_coe; /**< CoE state machine. */
ec_fsm_pdo_assign_t fsm_pdo_assign; /**< Pdo assignment state machine. */
ec_fsm_pdo_mapping_t fsm_pdo_mapping; /**< Pdo mapping state machine. */
};
/*****************************************************************************/
void ec_fsm_slave_config_init(ec_fsm_slave_config_t *, ec_datagram_t *);
void ec_fsm_slave_config_init(ec_fsm_slave_config_t *, ec_datagram_t *,
ec_fsm_change_t *, ec_fsm_coe_t *, ec_fsm_pdo_t *);
void ec_fsm_slave_config_clear(ec_fsm_slave_config_t *);
void ec_fsm_slave_config_start(ec_fsm_slave_config_t *, ec_slave_t *);

15
master/fsm_slave_scan.c
View File

@ -72,13 +72,12 @@ void ec_fsm_slave_scan_init(
ec_datagram_t *datagram, /**< Datagram to use. */
ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration
state machine to use. */
ec_fsm_coe_map_t *fsm_coe_map /**< Pdo mapping state machine to use.
*/
ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration machine to use. */
)
{
fsm->datagram = datagram;
fsm->fsm_slave_config = fsm_slave_config;
fsm->fsm_coe_map = fsm_coe_map;
fsm->fsm_pdo = fsm_pdo;
// init sub state machines
ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);
@ -633,8 +632,8 @@ void ec_fsm_slave_scan_enter_pdos(
EC_DBG("Scanning Pdo assignment and mapping of slave %u.\n",
slave->ring_position);
fsm->state = ec_fsm_slave_scan_state_pdos;
ec_fsm_coe_map_start(fsm->fsm_coe_map, slave);
ec_fsm_coe_map_exec(fsm->fsm_coe_map); // execute immediately
ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave);
ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately
}
/*****************************************************************************/
@ -645,15 +644,15 @@ void ec_fsm_slave_scan_state_pdos(
ec_fsm_slave_scan_t *fsm /**< slave state machine */
)
{
if (ec_fsm_coe_map_exec(fsm->fsm_coe_map))
if (ec_fsm_pdo_exec(fsm->fsm_pdo))
return;
if (!ec_fsm_coe_map_success(fsm->fsm_coe_map)) {
if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
fsm->state = ec_fsm_slave_scan_state_error;
return;
}
// fetching of Pdo assignment/mapping finished
// reading Pdo configuration finished
fsm->state = ec_fsm_slave_scan_state_end;
}

6
master/fsm_slave_scan.h
View File

@ -49,7 +49,7 @@
#include "fsm_sii.h"
#include "fsm_change.h"
#include "fsm_coe.h"
#include "fsm_coe_map.h"
#include "fsm_pdo.h"
/*****************************************************************************/
@ -64,7 +64,7 @@ struct ec_fsm_slave_scan
ec_datagram_t *datagram; /**< Datagram used in the state machine. */
ec_fsm_slave_config_t *fsm_slave_config; /**< Slave configuration state
machine to use. */
ec_fsm_coe_map_t *fsm_coe_map; /**< Pdo mapping state machine to use. */
ec_fsm_pdo_t *fsm_pdo; /**< Pdo configuration state machine to use. */
unsigned int retries; /**< Retries on datagram timeout. */
void (*state)(ec_fsm_slave_scan_t *); /**< State function. */
@ -76,7 +76,7 @@ struct ec_fsm_slave_scan
/*****************************************************************************/
void ec_fsm_slave_scan_init(ec_fsm_slave_scan_t *, ec_datagram_t *,
ec_fsm_slave_config_t *, ec_fsm_coe_map_t *);
ec_fsm_slave_config_t *, ec_fsm_pdo_t *);
void ec_fsm_slave_scan_clear(ec_fsm_slave_scan_t *);
void ec_fsm_slave_scan_start(ec_fsm_slave_scan_t *, ec_slave_t *);