# HG changeset patch # User Florian Pose # Date 1154521525 0 # Node ID 9dc190591c0fda87757fb506927f108e51bba66c # Parent 05622513f6272f22f0a8d0bb136ad839ddec63c8 MERGE trunk -r452:489 -> stable-1.0 (datagrams, bugfixes, EEPROM info, idle mode) diff -r 05622513f627 -r 9dc190591c0f FEATURES --- a/FEATURES Mon Jun 26 16:07:07 2006 +0000 +++ b/FEATURES Wed Aug 02 12:25:25 2006 +0000 @@ -38,7 +38,7 @@ operation. - Controlling of slave states during realtime operation. -* Free-Run mode, if master is idle. +* Special Idle mode, when master is not in use. - Automatic scanning of slaves upon topology changes. - Bus visualisation and EoE processing without realtime process connected. diff -r 05622513f627 -r 9dc190591c0f Makefile --- a/Makefile Mon Jun 26 16:07:07 2006 +0000 +++ b/Makefile Wed Aug 02 12:25:25 2006 +0000 @@ -69,6 +69,7 @@ clean: cleandoc $(MAKE) -C $(KERNEL_DIR) M=$(CURRENT_DIR) clean + @rm -f Modules.symvers doc: doxygen Doxyfile diff -r 05622513f627 -r 9dc190591c0f README --- a/README Mon Jun 26 16:07:07 2006 +0000 +++ b/README Wed Aug 02 12:25:25 2006 +0000 @@ -60,8 +60,8 @@ Realtime patches for the Linux kernel are supported, but not required. The realtime processing has to be done by the calling module (see API -documentation). The EtherCAT master code itself is (except for the free-run -mode) completely passive. +documentation). The EtherCAT master code itself is completely passive (except +for the idle mode). To avoid frame timeouts, deactivating DMA access for hard drives is recommented (hdparm -d0 ). diff -r 05622513f627 -r 9dc190591c0f devices/Makefile --- a/devices/Makefile Mon Jun 26 16:07:07 2006 +0000 +++ b/devices/Makefile Wed Aug 02 12:25:25 2006 +0000 @@ -74,6 +74,7 @@ clean: $(MAKE) -C $(KERNEL_DIR) M=$(CURRENT_DIR) clean + @rm -f Modules.symvers #------------------------------------------------------------------------------ diff -r 05622513f627 -r 9dc190591c0f include/ecrt.h --- a/include/ecrt.h Mon Jun 26 16:07:07 2006 +0000 +++ b/include/ecrt.h Wed Aug 02 12:25:25 2006 +0000 @@ -211,7 +211,7 @@ */ #define EC_READ_S8(DATA) \ - ((int8_t) *((uint8_t *) (DATA))) + ((int8_t) *((uint8_t *) (DATA))) /** Read a 16-bit unsigned value from EtherCAT data. @@ -229,7 +229,7 @@ */ #define EC_READ_S16(DATA) \ - ((int16_t) le16_to_cpup((void *) (DATA))) + ((int16_t) le16_to_cpup((void *) (DATA))) /** Read a 32-bit unsigned value from EtherCAT data. @@ -247,7 +247,7 @@ */ #define EC_READ_S32(DATA) \ - ((int32_t) le32_to_cpup((void *) (DATA))) + ((int32_t) le32_to_cpup((void *) (DATA))) /****************************************************************************** diff -r 05622513f627 -r 9dc190591c0f master/Makefile --- a/master/Makefile Mon Jun 26 16:07:07 2006 +0000 +++ b/master/Makefile Wed Aug 02 12:25:25 2006 +0000 @@ -42,7 +42,7 @@ obj-m := ec_master.o -ec_master-objs := module.o master.o device.o slave.o command.o types.o \ +ec_master-objs := module.o master.o device.o slave.o datagram.o types.o \ domain.o mailbox.o canopen.o ethernet.o debug.o fsm.o REV := $(shell svnversion $(src) 2>/dev/null || echo "unknown") @@ -75,6 +75,7 @@ clean: $(MAKE) -C $(KERNEL_DIR) M=$(CURRENT_DIR) clean + @rm -f Modules.symvers #------------------------------------------------------------------------------ diff -r 05622513f627 -r 9dc190591c0f master/canopen.c --- a/master/canopen.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/canopen.c Wed Aug 02 12:25:25 2006 +0000 @@ -48,8 +48,8 @@ /*****************************************************************************/ void ec_canopen_abort_msg(uint32_t); -int ec_slave_fetch_sdo_descriptions(ec_slave_t *, ec_command_t *); -int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_command_t *, +int ec_slave_fetch_sdo_descriptions(ec_slave_t *, ec_datagram_t *); +int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_datagram_t *, ec_sdo_t *, uint8_t); /*****************************************************************************/ @@ -65,13 +65,13 @@ uint8_t *target /**< 4-byte memory */ ) { - ec_command_t command; + ec_datagram_t datagram; size_t rec_size; uint8_t *data; - ec_command_init(&command); - - if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 6))) + ec_datagram_init(&datagram); + + if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6))) goto err; EC_WRITE_U16(data, 0x2 << 12); // SDO request @@ -80,7 +80,7 @@ EC_WRITE_U16(data + 3, sdo_index); EC_WRITE_U8 (data + 5, sdo_subindex); - if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size))) + if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size))) goto err; if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request @@ -104,10 +104,10 @@ memcpy(target, data + 6, 4); - ec_command_clear(&command); + ec_datagram_clear(&datagram); return 0; err: - ec_command_clear(&command); + ec_datagram_clear(&datagram); return -1; } @@ -127,16 +127,16 @@ { uint8_t *data; size_t rec_size; - ec_command_t command; - - ec_command_init(&command); + ec_datagram_t datagram; + + ec_datagram_init(&datagram); if (size == 0 || size > 4) { EC_ERR("Invalid data size!\n"); goto err; } - if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 0x0A))) + if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 0x0A))) goto err; EC_WRITE_U16(data, 0x2 << 12); // SDO request @@ -149,7 +149,7 @@ memcpy(data + 6, sdo_data, size); if (size < 4) memset(data + 6 + size, 0x00, 4 - size); - if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size))) + if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size))) goto err; if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request @@ -172,10 +172,10 @@ return -1; } - ec_command_clear(&command); + ec_datagram_clear(&datagram); return 0; err: - ec_command_clear(&command); + ec_datagram_clear(&datagram); return -1; } @@ -198,11 +198,11 @@ uint8_t *data; size_t rec_size, data_size; uint32_t complete_size; - ec_command_t command; - - ec_command_init(&command); - - if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 6))) + ec_datagram_t datagram; + + ec_datagram_init(&datagram); + + if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6))) goto err; EC_WRITE_U16(data, 0x2 << 12); // SDO request @@ -210,7 +210,7 @@ EC_WRITE_U16(data + 3, sdo_index); EC_WRITE_U8 (data + 5, sdo_subindex); - if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size))) + if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size))) goto err; if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request @@ -253,10 +253,10 @@ memcpy(target, data + 10, data_size); - ec_command_clear(&command); + ec_datagram_clear(&datagram); return 0; err: - ec_command_clear(&command); + ec_datagram_clear(&datagram); return -1; } @@ -274,11 +274,11 @@ unsigned int i, sdo_count; ec_sdo_t *sdo; uint16_t sdo_index; - ec_command_t command; - - ec_command_init(&command); - - if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 8))) + ec_datagram_t datagram; + + ec_datagram_init(&datagram); + + if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 8))) goto err; EC_WRITE_U16(data, 0x8 << 12); // SDO information @@ -287,13 +287,13 @@ EC_WRITE_U16(data + 4, 0x0000); EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! - if (unlikely(ec_master_simple_io(slave->master, &command))) { + if (unlikely(ec_master_simple_io(slave->master, &datagram))) { EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position); goto err; } do { - if (!(data = ec_slave_mbox_simple_receive(slave, &command, + if (!(data = ec_slave_mbox_simple_receive(slave, &datagram, 0x03, &rec_size))) goto err; @@ -342,12 +342,12 @@ while (EC_READ_U8(data + 2) & 0x80); // Fetch all SDO descriptions - if (ec_slave_fetch_sdo_descriptions(slave, &command)) goto err; - - ec_command_clear(&command); + if (ec_slave_fetch_sdo_descriptions(slave, &datagram)) goto err; + + ec_datagram_clear(&datagram); return 0; err: - ec_command_clear(&command); + ec_datagram_clear(&datagram); return -1; } @@ -359,7 +359,7 @@ */ int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave, /**< EtherCAT slave */ - ec_command_t *command /**< command */ + ec_datagram_t *datagram /**< datagram */ ) { uint8_t *data; @@ -367,7 +367,7 @@ ec_sdo_t *sdo; list_for_each_entry(sdo, &slave->sdo_dictionary, list) { - if (!(data = ec_slave_mbox_prepare_send(slave, command, 0x03, 8))) + if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) return -1; EC_WRITE_U16(data, 0x8 << 12); // SDO information EC_WRITE_U8 (data + 2, 0x03); // Get object description request @@ -375,7 +375,7 @@ EC_WRITE_U16(data + 4, 0x0000); EC_WRITE_U16(data + 6, sdo->index); // SDO index - if (!(data = ec_slave_mbox_simple_io(slave, command, &rec_size))) + if (!(data = ec_slave_mbox_simple_io(slave, datagram, &rec_size))) return -1; if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information @@ -428,7 +428,7 @@ } // Fetch all entries (subindices) - if (ec_slave_fetch_sdo_entries(slave, command, sdo, + if (ec_slave_fetch_sdo_entries(slave, datagram, sdo, EC_READ_U8(data + 10))) return -1; } @@ -444,7 +444,7 @@ */ int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT slave */ - ec_command_t *command, /**< command */ + ec_datagram_t *datagram, /**< datagram */ ec_sdo_t *sdo, /**< SDO */ uint8_t subindices /**< number of subindices */ ) @@ -455,7 +455,7 @@ ec_sdo_entry_t *entry; for (i = 1; i <= subindices; i++) { - if (!(data = ec_slave_mbox_prepare_send(slave, command, 0x03, 10))) + if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) return -1; EC_WRITE_U16(data, 0x8 << 12); // SDO information @@ -466,7 +466,7 @@ EC_WRITE_U8 (data + 8, i); // SDO subindex EC_WRITE_U8 (data + 9, 0x00); // value info (no values) - if (!(data = ec_slave_mbox_simple_io(slave, command, &rec_size))) + if (!(data = ec_slave_mbox_simple_io(slave, datagram, &rec_size))) return -1; if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information diff -r 05622513f627 -r 9dc190591c0f master/command.c --- a/master/command.c Mon Jun 26 16:07:07 2006 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,297 +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 - Methods of an EtherCAT command. -*/ - -/*****************************************************************************/ - -#include - -#include "command.h" -#include "master.h" - -/*****************************************************************************/ - -/** \cond */ - -#define EC_FUNC_HEADER \ - if (unlikely(ec_command_prealloc(command, data_size))) \ - return -1; \ - command->index = 0; \ - command->working_counter = 0; \ - command->state = EC_CMD_INIT; - -#define EC_FUNC_FOOTER \ - command->data_size = data_size; \ - memset(command->data, 0x00, data_size); \ - return 0; - -/** \endcond */ - -/*****************************************************************************/ - -/** - Command constructor. -*/ - -void ec_command_init(ec_command_t *command /**< EtherCAT command */) -{ - command->type = EC_CMD_NONE; - command->address.logical = 0x00000000; - command->data = NULL; - command->mem_size = 0; - command->data_size = 0; - command->index = 0x00; - command->working_counter = 0x00; - command->state = EC_CMD_INIT; - command->t_sent = 0; -} - -/*****************************************************************************/ - -/** - Command destructor. -*/ - -void ec_command_clear(ec_command_t *command /**< EtherCAT command */) -{ - if (command->data) kfree(command->data); -} - -/*****************************************************************************/ - -/** - Allocates command data memory. - If the allocated memory is already larger than requested, nothing ist done. - \return 0 in case of success, else < 0 -*/ - -int ec_command_prealloc(ec_command_t *command, /**< EtherCAT command */ - size_t size /**< New size in bytes */ - ) -{ - if (size <= command->mem_size) return 0; - - if (command->data) { - kfree(command->data); - command->data = NULL; - command->mem_size = 0; - } - - if (!(command->data = kmalloc(size, GFP_KERNEL))) { - EC_ERR("Failed to allocate %i bytes of command memory!\n", size); - return -1; - } - - command->mem_size = size; - return 0; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT NPRD command. - Node-adressed physical read. - \return 0 in case of success, else < 0 -*/ - -int ec_command_nprd(ec_command_t *command, - /**< EtherCAT command */ - uint16_t node_address, - /**< configured station address */ - uint16_t offset, - /**< physical memory address */ - size_t data_size - /**< number of bytes to read */ - ) -{ - if (unlikely(node_address == 0x0000)) - EC_WARN("Using node address 0x0000!\n"); - - EC_FUNC_HEADER; - command->type = EC_CMD_NPRD; - command->address.physical.slave = node_address; - command->address.physical.mem = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT NPWR command. - Node-adressed physical write. - \return 0 in case of success, else < 0 -*/ - -int ec_command_npwr(ec_command_t *command, - /**< EtherCAT command */ - uint16_t node_address, - /**< configured station address */ - uint16_t offset, - /**< physical memory address */ - size_t data_size - /**< number of bytes to write */ - ) -{ - if (unlikely(node_address == 0x0000)) - EC_WARN("Using node address 0x0000!\n"); - - EC_FUNC_HEADER; - command->type = EC_CMD_NPWR; - command->address.physical.slave = node_address; - command->address.physical.mem = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT APRD command. - Autoincrement physical read. - \return 0 in case of success, else < 0 -*/ - -int ec_command_aprd(ec_command_t *command, - /**< EtherCAT command */ - uint16_t ring_position, - /**< auto-increment position */ - uint16_t offset, - /**< physical memory address */ - size_t data_size - /**< number of bytes to read */ - ) -{ - EC_FUNC_HEADER; - command->type = EC_CMD_APRD; - command->address.physical.slave = (int16_t) ring_position * (-1); - command->address.physical.mem = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT APWR command. - Autoincrement physical write. - \return 0 in case of success, else < 0 -*/ - -int ec_command_apwr(ec_command_t *command, - /**< EtherCAT command */ - uint16_t ring_position, - /**< auto-increment position */ - uint16_t offset, - /**< physical memory address */ - size_t data_size - /**< number of bytes to write */ - ) -{ - EC_FUNC_HEADER; - command->type = EC_CMD_APWR; - command->address.physical.slave = (int16_t) ring_position * (-1); - command->address.physical.mem = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT BRD command. - Broadcast read. - \return 0 in case of success, else < 0 -*/ - -int ec_command_brd(ec_command_t *command, - /**< EtherCAT command */ - uint16_t offset, - /**< physical memory address */ - size_t data_size - /**< number of bytes to read */ - ) -{ - EC_FUNC_HEADER; - command->type = EC_CMD_BRD; - command->address.physical.slave = 0x0000; - command->address.physical.mem = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT BWR command. - Broadcast write. - \return 0 in case of success, else < 0 -*/ - -int ec_command_bwr(ec_command_t *command, - /**< EtherCAT command */ - uint16_t offset, - /**< physical memory address */ - size_t data_size - /**< number of bytes to write */ - ) -{ - EC_FUNC_HEADER; - command->type = EC_CMD_BWR; - command->address.physical.slave = 0x0000; - command->address.physical.mem = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ - -/** - Initializes an EtherCAT LRW command. - Logical read write. - \return 0 in case of success, else < 0 -*/ - -int ec_command_lrw(ec_command_t *command, - /**< EtherCAT command */ - uint32_t offset, - /**< logical address */ - size_t data_size - /**< number of bytes to read/write */ - ) -{ - EC_FUNC_HEADER; - command->type = EC_CMD_LRW; - command->address.logical = offset; - EC_FUNC_FOOTER; -} - -/*****************************************************************************/ diff -r 05622513f627 -r 9dc190591c0f master/command.h --- a/master/command.h Mon Jun 26 16:07:07 2006 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,140 +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 command structure. -*/ - -/*****************************************************************************/ - -#ifndef _EC_COMMAND_H_ -#define _EC_COMMAND_H_ - -#include -#include - -#include "globals.h" - -/*****************************************************************************/ - -/** - EtherCAT command type. -*/ - -typedef enum -{ - EC_CMD_NONE = 0x00, /**< Dummy */ - EC_CMD_APRD = 0x01, /**< Auto-increment physical read */ - EC_CMD_APWR = 0x02, /**< Auto-increment physical write */ - EC_CMD_NPRD = 0x04, /**< Node-addressed physical read */ - EC_CMD_NPWR = 0x05, /**< Node-addressed physical write */ - EC_CMD_BRD = 0x07, /**< Broadcast read */ - EC_CMD_BWR = 0x08, /**< Broadcast write */ - EC_CMD_LRW = 0x0C /**< Logical read/write */ -} -ec_command_type_t; - -/** - EtherCAT command state. -*/ - -typedef enum -{ - EC_CMD_INIT, /**< new command */ - EC_CMD_QUEUED, /**< command queued by master */ - EC_CMD_SENT, /**< command has been sent */ - EC_CMD_RECEIVED, /**< command has been received */ - EC_CMD_TIMEOUT, /**< command timed out */ - EC_CMD_ERROR /**< error while sending/receiving */ -} -ec_command_state_t; - -/*****************************************************************************/ - -/** - EtherCAT address. -*/ - -typedef union -{ - struct - { - uint16_t slave; /**< configured or autoincrement address */ - uint16_t mem; /**< physical memory address */ - } - physical; /**< physical address */ - - uint32_t logical; /**< logical address */ -} -ec_address_t; - -/*****************************************************************************/ - -/** - EtherCAT command. -*/ - -typedef struct -{ - struct list_head list; /**< needed by domain command lists */ - struct list_head queue; /**< master command queue item */ - ec_command_type_t type; /**< command type (APRD, BWR, etc) */ - ec_address_t address; /**< receipient address */ - uint8_t *data; /**< command data */ - size_t mem_size; /**< command \a data memory size */ - size_t data_size; /**< size of the data in \a data */ - uint8_t index; /**< command index (set by master) */ - uint16_t working_counter; /**< working counter */ - ec_command_state_t state; /**< command state */ - cycles_t t_sent; /**< time, the commands was sent */ -} -ec_command_t; - -/*****************************************************************************/ - -void ec_command_init(ec_command_t *); -void ec_command_clear(ec_command_t *); -int ec_command_prealloc(ec_command_t *, size_t); - -int ec_command_nprd(ec_command_t *, uint16_t, uint16_t, size_t); -int ec_command_npwr(ec_command_t *, uint16_t, uint16_t, size_t); -int ec_command_aprd(ec_command_t *, uint16_t, uint16_t, size_t); -int ec_command_apwr(ec_command_t *, uint16_t, uint16_t, size_t); -int ec_command_brd(ec_command_t *, uint16_t, size_t); -int ec_command_bwr(ec_command_t *, uint16_t, size_t); -int ec_command_lrw(ec_command_t *, uint32_t, size_t); - -/*****************************************************************************/ - -#endif diff -r 05622513f627 -r 9dc190591c0f master/datagram.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/datagram.c Wed Aug 02 12:25:25 2006 +0000 @@ -0,0 +1,297 @@ +/****************************************************************************** + * + * $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 + Methods of an EtherCAT datagram. +*/ + +/*****************************************************************************/ + +#include + +#include "datagram.h" +#include "master.h" + +/*****************************************************************************/ + +/** \cond */ + +#define EC_FUNC_HEADER \ + if (unlikely(ec_datagram_prealloc(datagram, data_size))) \ + return -1; \ + datagram->index = 0; \ + datagram->working_counter = 0; \ + datagram->state = EC_CMD_INIT; + +#define EC_FUNC_FOOTER \ + datagram->data_size = data_size; \ + memset(datagram->data, 0x00, data_size); \ + return 0; + +/** \endcond */ + +/*****************************************************************************/ + +/** + Datagram constructor. +*/ + +void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram */) +{ + datagram->type = EC_CMD_NONE; + datagram->address.logical = 0x00000000; + datagram->data = NULL; + datagram->mem_size = 0; + datagram->data_size = 0; + datagram->index = 0x00; + datagram->working_counter = 0x00; + datagram->state = EC_CMD_INIT; + datagram->t_sent = 0; +} + +/*****************************************************************************/ + +/** + Datagram destructor. +*/ + +void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram */) +{ + if (datagram->data) kfree(datagram->data); +} + +/*****************************************************************************/ + +/** + Allocates datagram data memory. + If the allocated memory is already larger than requested, nothing ist done. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_prealloc(ec_datagram_t *datagram, /**< EtherCAT datagram */ + size_t size /**< New size in bytes */ + ) +{ + if (size <= datagram->mem_size) return 0; + + if (datagram->data) { + kfree(datagram->data); + datagram->data = NULL; + datagram->mem_size = 0; + } + + if (!(datagram->data = kmalloc(size, GFP_KERNEL))) { + EC_ERR("Failed to allocate %i bytes of datagram memory!\n", size); + return -1; + } + + datagram->mem_size = size; + return 0; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT NPRD datagram. + Node-adressed physical read. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_nprd(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint16_t node_address, + /**< configured station address */ + uint16_t offset, + /**< physical memory address */ + size_t data_size + /**< number of bytes to read */ + ) +{ + if (unlikely(node_address == 0x0000)) + EC_WARN("Using node address 0x0000!\n"); + + EC_FUNC_HEADER; + datagram->type = EC_CMD_NPRD; + datagram->address.physical.slave = node_address; + datagram->address.physical.mem = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT NPWR datagram. + Node-adressed physical write. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_npwr(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint16_t node_address, + /**< configured station address */ + uint16_t offset, + /**< physical memory address */ + size_t data_size + /**< number of bytes to write */ + ) +{ + if (unlikely(node_address == 0x0000)) + EC_WARN("Using node address 0x0000!\n"); + + EC_FUNC_HEADER; + datagram->type = EC_CMD_NPWR; + datagram->address.physical.slave = node_address; + datagram->address.physical.mem = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT APRD datagram. + Autoincrement physical read. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_aprd(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint16_t ring_position, + /**< auto-increment position */ + uint16_t offset, + /**< physical memory address */ + size_t data_size + /**< number of bytes to read */ + ) +{ + EC_FUNC_HEADER; + datagram->type = EC_CMD_APRD; + datagram->address.physical.slave = (int16_t) ring_position * (-1); + datagram->address.physical.mem = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT APWR datagram. + Autoincrement physical write. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_apwr(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint16_t ring_position, + /**< auto-increment position */ + uint16_t offset, + /**< physical memory address */ + size_t data_size + /**< number of bytes to write */ + ) +{ + EC_FUNC_HEADER; + datagram->type = EC_CMD_APWR; + datagram->address.physical.slave = (int16_t) ring_position * (-1); + datagram->address.physical.mem = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT BRD datagram. + Broadcast read. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_brd(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint16_t offset, + /**< physical memory address */ + size_t data_size + /**< number of bytes to read */ + ) +{ + EC_FUNC_HEADER; + datagram->type = EC_CMD_BRD; + datagram->address.physical.slave = 0x0000; + datagram->address.physical.mem = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT BWR datagram. + Broadcast write. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_bwr(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint16_t offset, + /**< physical memory address */ + size_t data_size + /**< number of bytes to write */ + ) +{ + EC_FUNC_HEADER; + datagram->type = EC_CMD_BWR; + datagram->address.physical.slave = 0x0000; + datagram->address.physical.mem = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ + +/** + Initializes an EtherCAT LRW datagram. + Logical read write. + \return 0 in case of success, else < 0 +*/ + +int ec_datagram_lrw(ec_datagram_t *datagram, + /**< EtherCAT datagram */ + uint32_t offset, + /**< logical address */ + size_t data_size + /**< number of bytes to read/write */ + ) +{ + EC_FUNC_HEADER; + datagram->type = EC_CMD_LRW; + datagram->address.logical = offset; + EC_FUNC_FOOTER; +} + +/*****************************************************************************/ diff -r 05622513f627 -r 9dc190591c0f master/datagram.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/datagram.h Wed Aug 02 12:25:25 2006 +0000 @@ -0,0 +1,141 @@ +/****************************************************************************** + * + * $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 datagram structure. +*/ + +/*****************************************************************************/ + +#ifndef _EC_DATAGRAM_H_ +#define _EC_DATAGRAM_H_ + +#include +#include +#include + +#include "globals.h" + +/*****************************************************************************/ + +/** + EtherCAT datagram type. +*/ + +typedef enum +{ + EC_CMD_NONE = 0x00, /**< Dummy */ + EC_CMD_APRD = 0x01, /**< Auto-increment physical read */ + EC_CMD_APWR = 0x02, /**< Auto-increment physical write */ + EC_CMD_NPRD = 0x04, /**< Node-addressed physical read */ + EC_CMD_NPWR = 0x05, /**< Node-addressed physical write */ + EC_CMD_BRD = 0x07, /**< Broadcast read */ + EC_CMD_BWR = 0x08, /**< Broadcast write */ + EC_CMD_LRW = 0x0C /**< Logical read/write */ +} +ec_datagram_type_t; + +/** + EtherCAT datagram state. +*/ + +typedef enum +{ + EC_CMD_INIT, /**< new datagram */ + EC_CMD_QUEUED, /**< datagram queued by master */ + EC_CMD_SENT, /**< datagram has been sent */ + EC_CMD_RECEIVED, /**< datagram has been received */ + EC_CMD_TIMEOUT, /**< datagram timed out */ + EC_CMD_ERROR /**< error while sending/receiving */ +} +ec_datagram_state_t; + +/*****************************************************************************/ + +/** + EtherCAT address. +*/ + +typedef union +{ + struct + { + uint16_t slave; /**< configured or autoincrement address */ + uint16_t mem; /**< physical memory address */ + } + physical; /**< physical address */ + + uint32_t logical; /**< logical address */ +} +ec_address_t; + +/*****************************************************************************/ + +/** + EtherCAT datagram. +*/ + +typedef struct +{ + struct list_head list; /**< needed by domain datagram lists */ + struct list_head queue; /**< master datagram queue item */ + ec_datagram_type_t type; /**< datagram type (APRD, BWR, etc) */ + ec_address_t address; /**< receipient address */ + uint8_t *data; /**< datagram data */ + size_t mem_size; /**< datagram \a data memory size */ + size_t data_size; /**< size of the data in \a data */ + uint8_t index; /**< datagram index (set by master) */ + uint16_t working_counter; /**< working counter */ + ec_datagram_state_t state; /**< datagram state */ + cycles_t t_sent; /**< time, the datagrams was sent */ +} +ec_datagram_t; + +/*****************************************************************************/ + +void ec_datagram_init(ec_datagram_t *); +void ec_datagram_clear(ec_datagram_t *); +int ec_datagram_prealloc(ec_datagram_t *, size_t); + +int ec_datagram_nprd(ec_datagram_t *, uint16_t, uint16_t, size_t); +int ec_datagram_npwr(ec_datagram_t *, uint16_t, uint16_t, size_t); +int ec_datagram_aprd(ec_datagram_t *, uint16_t, uint16_t, size_t); +int ec_datagram_apwr(ec_datagram_t *, uint16_t, uint16_t, size_t); +int ec_datagram_brd(ec_datagram_t *, uint16_t, size_t); +int ec_datagram_bwr(ec_datagram_t *, uint16_t, size_t); +int ec_datagram_lrw(ec_datagram_t *, uint32_t, size_t); + +/*****************************************************************************/ + +#endif diff -r 05622513f627 -r 9dc190591c0f master/debug.c --- a/master/debug.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/debug.c Wed Aug 02 12:25:25 2006 +0000 @@ -38,6 +38,7 @@ /*****************************************************************************/ +#include #include #include "globals.h" diff -r 05622513f627 -r 9dc190591c0f master/domain.c --- a/master/domain.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/domain.c Wed Aug 02 12:25:25 2006 +0000 @@ -38,6 +38,8 @@ /*****************************************************************************/ +#include + #include "globals.h" #include "domain.h" #include "master.h" @@ -51,10 +53,10 @@ /** \cond */ -EC_SYSFS_READ_ATTR(data_size); +EC_SYSFS_READ_ATTR(image_size); static struct attribute *def_attrs[] = { - &attr_data_size, + &attr_image_size, NULL, }; @@ -90,7 +92,7 @@ domain->response_count = 0xFFFFFFFF; INIT_LIST_HEAD(&domain->field_regs); - INIT_LIST_HEAD(&domain->commands); + INIT_LIST_HEAD(&domain->datagrams); // init kobject and add it to the hierarchy memset(&domain->kobj, 0x00, sizeof(struct kobject)); @@ -113,16 +115,16 @@ void ec_domain_clear(struct kobject *kobj /**< kobject of the domain */) { - ec_command_t *command, *next; + ec_datagram_t *datagram, *next; ec_domain_t *domain; domain = container_of(kobj, ec_domain_t, kobj); EC_INFO("Clearing domain %i.\n", domain->index); - list_for_each_entry_safe(command, next, &domain->commands, list) { - ec_command_clear(command); - kfree(command); + list_for_each_entry_safe(datagram, next, &domain->datagrams, list) { + ec_datagram_clear(datagram); + kfree(datagram); } ec_domain_clear_field_regs(domain); @@ -187,30 +189,30 @@ /*****************************************************************************/ /** - Allocates a process data command and appends it to the list. + Allocates a process data datagram and appends it to the list. \return 0 in case of success, else < 0 */ -int ec_domain_add_command(ec_domain_t *domain, /**< EtherCAT domain */ - uint32_t offset, /**< logical offset */ - size_t data_size /**< size of the command data */ - ) -{ - ec_command_t *command; - - if (!(command = kmalloc(sizeof(ec_command_t), GFP_KERNEL))) { - EC_ERR("Failed to allocate domain command!\n"); +int ec_domain_add_datagram(ec_domain_t *domain, /**< EtherCAT domain */ + uint32_t offset, /**< logical offset */ + size_t data_size /**< size of the datagram data */ + ) +{ + ec_datagram_t *datagram; + + if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate domain datagram!\n"); return -1; } - ec_command_init(command); - - if (ec_command_lrw(command, offset, data_size)) { - kfree(command); + ec_datagram_init(datagram); + + if (ec_datagram_lrw(datagram, offset, data_size)) { + kfree(datagram); return -1; } - list_add_tail(&command->list, &domain->commands); + list_add_tail(&datagram->list, &domain->datagrams); return 0; } @@ -235,7 +237,7 @@ uint32_t field_off, field_off_cmd; uint32_t cmd_offset; size_t cmd_data_size, sync_size; - ec_command_t *command; + ec_datagram_t *datagram; domain->base_address = base_address; @@ -252,8 +254,8 @@ sync_size = ec_slave_calc_sync_size(slave, fmmu->sync); domain->data_size += sync_size; if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) { - if (ec_domain_add_command(domain, cmd_offset, - cmd_data_size)) return -1; + if (ec_domain_add_datagram(domain, cmd_offset, + cmd_data_size)) return -1; cmd_offset += cmd_data_size; cmd_data_size = 0; cmd_count++; @@ -263,9 +265,9 @@ } } - // allocate last command + // allocate last datagram if (cmd_data_size) { - if (ec_domain_add_command(domain, cmd_offset, cmd_data_size)) + if (ec_domain_add_datagram(domain, cmd_offset, cmd_data_size)) return -1; cmd_count++; } @@ -283,12 +285,12 @@ if (fmmu->domain == domain && fmmu->sync == field_reg->sync) { field_off = fmmu->logical_start_address + field_reg->field_offset; - // search command - list_for_each_entry(command, &domain->commands, list) { - field_off_cmd = field_off - command->address.logical; - if (field_off >= command->address.logical && - field_off_cmd < command->mem_size) { - *field_reg->data_ptr = command->data + field_off_cmd; + // search datagram + list_for_each_entry(datagram, &domain->datagrams, list) { + field_off_cmd = field_off - datagram->address.logical; + if (field_off >= datagram->address.logical && + field_off_cmd < datagram->mem_size) { + *field_reg->data_ptr = datagram->data + field_off_cmd; } } if (!field_reg->data_ptr) { @@ -300,7 +302,7 @@ } } - EC_INFO("Domain %i - Allocated %i bytes in %i command%s\n", + EC_INFO("Domain %i - Allocated %i bytes in %i datagram%s\n", domain->index, domain->data_size, cmd_count, cmd_count == 1 ? "" : "s"); @@ -314,7 +316,7 @@ /** Sets the number of responding slaves and outputs it on demand. This number isn't really the number of responding slaves, but the sum of - the working counters of all domain commands. Some slaves increase the + the working counters of all domain datagrams. Some slaves increase the working counter by 2, some by 1. */ @@ -343,7 +345,7 @@ { ec_domain_t *domain = container_of(kobj, ec_domain_t, kobj); - if (attr == &attr_data_size) { + if (attr == &attr_image_size) { return sprintf(buffer, "%i\n", domain->data_size); } @@ -479,16 +481,16 @@ /*****************************************************************************/ /** - Places all process data commands in the masters command queue. + Places all process data datagrams in the masters datagram queue. \ingroup RealtimeInterface */ void ecrt_domain_queue(ec_domain_t *domain /**< EtherCAT domain */) { - ec_command_t *command; - - list_for_each_entry(command, &domain->commands, list) { - ec_master_queue_command(domain->master, command); + ec_datagram_t *datagram; + + list_for_each_entry(datagram, &domain->datagrams, list) { + ec_master_queue_datagram(domain->master, datagram); } } @@ -502,13 +504,13 @@ void ecrt_domain_process(ec_domain_t *domain /**< EtherCAT domain */) { unsigned int working_counter_sum; - ec_command_t *command; + ec_datagram_t *datagram; working_counter_sum = 0; - list_for_each_entry(command, &domain->commands, list) { - if (command->state == EC_CMD_RECEIVED) { - working_counter_sum += command->working_counter; + list_for_each_entry(datagram, &domain->datagrams, list) { + if (datagram->state == EC_CMD_RECEIVED) { + working_counter_sum += datagram->working_counter; } } @@ -519,16 +521,16 @@ /** Returns the state of a domain. - \return 0 if all commands were received, else -1. + \return 0 if all datagrams were received, else -1. \ingroup RealtimeInterface */ int ecrt_domain_state(ec_domain_t *domain /**< EtherCAT domain */) { - ec_command_t *command; - - list_for_each_entry(command, &domain->commands, list) { - if (command->state != EC_CMD_RECEIVED) return -1; + ec_datagram_t *datagram; + + list_for_each_entry(datagram, &domain->datagrams, list) { + if (datagram->state != EC_CMD_RECEIVED) return -1; } return 0; diff -r 05622513f627 -r 9dc190591c0f master/domain.h --- a/master/domain.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/domain.h Wed Aug 02 12:25:25 2006 +0000 @@ -46,7 +46,7 @@ #include "globals.h" #include "slave.h" -#include "command.h" +#include "datagram.h" /*****************************************************************************/ @@ -68,7 +68,7 @@ /** EtherCAT domain. - Handles the process data and the therefore needed commands of a certain + Handles the process data and the therefore needed datagrams of a certain group of slaves. */ @@ -79,7 +79,7 @@ unsigned int index; /**< domain index (just a number) */ ec_master_t *master; /**< EtherCAT master owning the domain */ size_t data_size; /**< size of the process data */ - struct list_head commands; /**< process data commands */ + struct list_head datagrams; /**< process data datagrams */ uint32_t base_address; /**< logical offset address of the process data */ unsigned int response_count; /**< number of responding slaves */ struct list_head field_regs; /**< data field registrations */ diff -r 05622513f627 -r 9dc190591c0f master/ethernet.c --- a/master/ethernet.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/ethernet.c Wed Aug 02 12:25:25 2006 +0000 @@ -38,6 +38,7 @@ /*****************************************************************************/ +#include #include #include "../include/ecrt.h" @@ -87,7 +88,7 @@ int result, i; eoe->slave = NULL; - ec_command_init(&eoe->command); + ec_datagram_init(&eoe->datagram); eoe->state = ec_eoe_state_rx_start; eoe->opened = 0; eoe->rx_skb = NULL; @@ -154,7 +155,7 @@ void ec_eoe_clear(ec_eoe_t *eoe /**< EoE handler */) { - if (eoe->dev) { + if (eoe->dev) { // TODO: dev never NULL? unregister_netdev(eoe->dev); free_netdev(eoe->dev); } @@ -169,7 +170,7 @@ if (eoe->rx_skb) dev_kfree_skb(eoe->rx_skb); - ec_command_clear(&eoe->command); + ec_datagram_clear(&eoe->datagram); } /*****************************************************************************/ @@ -224,6 +225,7 @@ complete_offset = eoe->tx_offset / 32; } else { + // complete size in 32 bit blocks, rounded up. complete_offset = remaining_size / 32 + 1; } @@ -246,7 +248,7 @@ printk("\n"); #endif - if (!(data = ec_slave_mbox_prepare_send(eoe->slave, &eoe->command, + if (!(data = ec_slave_mbox_prepare_send(eoe->slave, &eoe->datagram, 0x02, current_size + 4))) return -1; @@ -257,7 +259,7 @@ (eoe->tx_frame_number & 0x0F) << 12)); memcpy(data + 4, eoe->tx_frame->skb->data + eoe->tx_offset, current_size); - ec_master_queue_command(eoe->slave->master, &eoe->command); + ec_master_queue_datagram(eoe->slave->master, &eoe->datagram); eoe->tx_offset += current_size; eoe->tx_fragment_number++; @@ -312,8 +314,8 @@ /** State: RX_START. - Starts a new receiving sequence by queueing a command that checks the - slave's mailbox for a new EoE command. + Starts a new receiving sequence by queueing a datagram that checks the + slave's mailbox for a new EoE datagram. */ void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */) @@ -321,8 +323,8 @@ if (!eoe->slave->online || !eoe->slave->master->device->link_state) return; - ec_slave_mbox_prepare_check(eoe->slave, &eoe->command); - ec_master_queue_command(eoe->slave->master, &eoe->command); + ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram); + ec_master_queue_datagram(eoe->slave->master, &eoe->datagram); eoe->state = ec_eoe_state_rx_check; } @@ -330,25 +332,25 @@ /** State: RX_CHECK. - Processes the checking command sent in RX_START and issues a receive - command, if new data is available. + Processes the checking datagram sent in RX_START and issues a receive + datagram, if new data is available. */ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */) { - if (eoe->command.state != EC_CMD_RECEIVED) { + if (eoe->datagram.state != EC_CMD_RECEIVED) { eoe->stats.rx_errors++; eoe->state = ec_eoe_state_tx_start; return; } - if (!ec_slave_mbox_check(&eoe->command)) { + if (!ec_slave_mbox_check(&eoe->datagram)) { eoe->state = ec_eoe_state_tx_start; return; } - ec_slave_mbox_prepare_fetch(eoe->slave, &eoe->command); - ec_master_queue_command(eoe->slave->master, &eoe->command); + ec_slave_mbox_prepare_fetch(eoe->slave, &eoe->datagram); + ec_master_queue_datagram(eoe->slave->master, &eoe->datagram); eoe->state = ec_eoe_state_rx_fetch; } @@ -357,7 +359,7 @@ /** State: RX_FETCH. Checks if the requested data of RX_CHECK was received and processes the - EoE command. + EoE datagram. */ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */) @@ -367,13 +369,13 @@ uint8_t frame_number, fragment_offset, fragment_number; off_t offset; - if (eoe->command.state != EC_CMD_RECEIVED) { + if (eoe->datagram.state != EC_CMD_RECEIVED) { eoe->stats.rx_errors++; eoe->state = ec_eoe_state_tx_start; return; } - if (!(data = ec_slave_mbox_fetch(eoe->slave, &eoe->command, + if (!(data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram, 0x02, &rec_size))) { eoe->stats.rx_errors++; eoe->state = ec_eoe_state_tx_start; @@ -382,114 +384,116 @@ frame_type = EC_READ_U16(data) & 0x000F; - if (frame_type == 0x00) { // EoE Fragment Request - last_fragment = (EC_READ_U16(data) >> 8) & 0x0001; - time_appended = (EC_READ_U16(data) >> 9) & 0x0001; - fragment_number = EC_READ_U16(data + 2) & 0x003F; - fragment_offset = (EC_READ_U16(data + 2) >> 6) & 0x003F; - frame_number = (EC_READ_U16(data + 2) >> 12) & 0x000F; - -#if EOE_DEBUG_LEVEL > 0 - EC_DBG("EoE RX fragment %i, offset %i, frame %i%s%s," - " %i octets\n", fragment_number, fragment_offset, - frame_number, - last_fragment ? ", last fragment" : "", - time_appended ? ", + timestamp" : "", - time_appended ? rec_size - 8 : rec_size - 4); -#endif - -#if EOE_DEBUG_LEVEL > 1 - EC_DBG(""); - for (i = 0; i < rec_size - 4; i++) { - printk("%02X ", data[i + 4]); - if ((i + 1) % 16 == 0) { - printk("\n"); - EC_DBG(""); - } - } - printk("\n"); -#endif - - data_size = time_appended ? rec_size - 8 : rec_size - 4; - - if (!fragment_number) { - if (eoe->rx_skb) { - EC_WARN("EoE RX freeing old socket buffer...\n"); - dev_kfree_skb(eoe->rx_skb); - } - - // new socket buffer - if (!(eoe->rx_skb = dev_alloc_skb(fragment_offset * 32))) { - if (printk_ratelimit()) - EC_WARN("EoE RX low on mem. frame dropped.\n"); - eoe->stats.rx_dropped++; - eoe->state = ec_eoe_state_tx_start; - return; - } - - eoe->rx_skb_offset = 0; - eoe->rx_skb_size = fragment_offset * 32; - eoe->rx_expected_fragment = 0; - } - else { - if (!eoe->rx_skb) { - eoe->stats.rx_dropped++; - eoe->state = ec_eoe_state_tx_start; - return; - } - - offset = fragment_offset * 32; - if (offset != eoe->rx_skb_offset || - offset + data_size > eoe->rx_skb_size || - fragment_number != eoe->rx_expected_fragment) { - eoe->stats.rx_errors++; - eoe->state = ec_eoe_state_tx_start; - dev_kfree_skb(eoe->rx_skb); - eoe->rx_skb = NULL; - return; - } - } - - // copy fragment into socket buffer - memcpy(skb_put(eoe->rx_skb, data_size), data + 4, data_size); - eoe->rx_skb_offset += data_size; - - if (last_fragment) { - // update statistics - eoe->stats.rx_packets++; - eoe->stats.rx_bytes += eoe->rx_skb->len; - -#if EOE_DEBUG_LEVEL > 0 - EC_DBG("EoE RX frame completed with %u octets.\n", - eoe->rx_skb->len); -#endif - - // pass socket buffer to network stack - eoe->rx_skb->dev = eoe->dev; - eoe->rx_skb->protocol = eth_type_trans(eoe->rx_skb, eoe->dev); - eoe->rx_skb->ip_summed = CHECKSUM_UNNECESSARY; - if (netif_rx(eoe->rx_skb)) { - EC_WARN("EoE RX netif_rx failed.\n"); - } - eoe->rx_skb = NULL; - - eoe->state = ec_eoe_state_tx_start; - } - else { - eoe->rx_expected_fragment++; -#if EOE_DEBUG_LEVEL > 0 - EC_DBG("EoE RX expecting fragment %i\n", - eoe->rx_expected_fragment); -#endif - eoe->state = ec_eoe_state_rx_start; - } - } - else { + if (frame_type != 0x00) { #if EOE_DEBUG_LEVEL > 0 EC_DBG("other frame received.\n"); #endif eoe->stats.rx_dropped++; eoe->state = ec_eoe_state_tx_start; + return; + } + + // EoE Fragment Request received + + last_fragment = (EC_READ_U16(data) >> 8) & 0x0001; + time_appended = (EC_READ_U16(data) >> 9) & 0x0001; + fragment_number = EC_READ_U16(data + 2) & 0x003F; + fragment_offset = (EC_READ_U16(data + 2) >> 6) & 0x003F; + frame_number = (EC_READ_U16(data + 2) >> 12) & 0x000F; + +#if EOE_DEBUG_LEVEL > 0 + EC_DBG("EoE RX fragment %i, offset %i, frame %i%s%s," + " %i octets\n", fragment_number, fragment_offset, + frame_number, + last_fragment ? ", last fragment" : "", + time_appended ? ", + timestamp" : "", + time_appended ? rec_size - 8 : rec_size - 4); +#endif + +#if EOE_DEBUG_LEVEL > 1 + EC_DBG(""); + for (i = 0; i < rec_size - 4; i++) { + printk("%02X ", data[i + 4]); + if ((i + 1) % 16 == 0) { + printk("\n"); + EC_DBG(""); + } + } + printk("\n"); +#endif + + data_size = time_appended ? rec_size - 8 : rec_size - 4; + + if (!fragment_number) { + if (eoe->rx_skb) { + EC_WARN("EoE RX freeing old socket buffer...\n"); + dev_kfree_skb(eoe->rx_skb); + } + + // new socket buffer + if (!(eoe->rx_skb = dev_alloc_skb(fragment_offset * 32))) { + if (printk_ratelimit()) + EC_WARN("EoE RX low on mem. frame dropped.\n"); + eoe->stats.rx_dropped++; + eoe->state = ec_eoe_state_tx_start; + return; + } + + eoe->rx_skb_offset = 0; + eoe->rx_skb_size = fragment_offset * 32; + eoe->rx_expected_fragment = 0; + } + else { + if (!eoe->rx_skb) { + eoe->stats.rx_dropped++; + eoe->state = ec_eoe_state_tx_start; + return; + } + + offset = fragment_offset * 32; + if (offset != eoe->rx_skb_offset || + offset + data_size > eoe->rx_skb_size || + fragment_number != eoe->rx_expected_fragment) { + dev_kfree_skb(eoe->rx_skb); + eoe->rx_skb = NULL; + eoe->stats.rx_errors++; + eoe->state = ec_eoe_state_tx_start; + return; + } + } + + // copy fragment into socket buffer + memcpy(skb_put(eoe->rx_skb, data_size), data + 4, data_size); + eoe->rx_skb_offset += data_size; + + if (last_fragment) { + // update statistics + eoe->stats.rx_packets++; + eoe->stats.rx_bytes += eoe->rx_skb->len; + +#if EOE_DEBUG_LEVEL > 0 + EC_DBG("EoE RX frame completed with %u octets.\n", + eoe->rx_skb->len); +#endif + + // pass socket buffer to network stack + eoe->rx_skb->dev = eoe->dev; + eoe->rx_skb->protocol = eth_type_trans(eoe->rx_skb, eoe->dev); + eoe->rx_skb->ip_summed = CHECKSUM_UNNECESSARY; + if (netif_rx(eoe->rx_skb)) { + EC_WARN("EoE RX netif_rx failed.\n"); + } + eoe->rx_skb = NULL; + + eoe->state = ec_eoe_state_tx_start; + } + else { + eoe->rx_expected_fragment++; +#if EOE_DEBUG_LEVEL > 0 + EC_DBG("EoE RX expecting fragment %i\n", + eoe->rx_expected_fragment); +#endif + eoe->state = ec_eoe_state_rx_start; } } @@ -560,19 +564,19 @@ /** State: TX SENT. - Checks is the previous transmit command succeded and sends the next + Checks is the previous transmit datagram succeded and sends the next fragment, if necessary. */ void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */) { - if (eoe->command.state != EC_CMD_RECEIVED) { + if (eoe->datagram.state != EC_CMD_RECEIVED) { eoe->stats.tx_errors++; eoe->state = ec_eoe_state_rx_start; return; } - if (eoe->command.working_counter != 1) { + if (eoe->datagram.working_counter != 1) { eoe->stats.tx_errors++; eoe->state = ec_eoe_state_rx_start; return; @@ -618,7 +622,7 @@ EC_WARN("device %s is not coupled to any EoE slave!\n", dev->name); else { eoe->slave->requested_state = EC_SLAVE_STATE_OP; - eoe->slave->state_error = 0; + eoe->slave->error_flag = 0; } return 0; } @@ -641,7 +645,7 @@ EC_WARN("device %s is not coupled to any EoE slave!\n", dev->name); else { eoe->slave->requested_state = EC_SLAVE_STATE_INIT; - eoe->slave->state_error = 0; + eoe->slave->error_flag = 0; } return 0; } diff -r 05622513f627 -r 9dc190591c0f master/ethernet.h --- a/master/ethernet.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/ethernet.h Wed Aug 02 12:25:25 2006 +0000 @@ -44,7 +44,7 @@ #include "../include/ecrt.h" #include "globals.h" #include "slave.h" -#include "command.h" +#include "datagram.h" /*****************************************************************************/ @@ -73,7 +73,7 @@ { struct list_head list; /**< list item */ ec_slave_t *slave; /**< pointer to the corresponding slave */ - ec_command_t command; /**< command */ + ec_datagram_t datagram; /**< datagram */ void (*state)(ec_eoe_t *); /**< state function for the state machine */ struct net_device *dev; /**< net_device for virtual ethernet device */ struct net_device_stats stats; /**< device statistics */ diff -r 05622513f627 -r 9dc190591c0f master/fsm.c --- a/master/fsm.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/fsm.c Wed Aug 02 12:25:25 2006 +0000 @@ -46,32 +46,30 @@ void ec_fsm_master_start(ec_fsm_t *); void ec_fsm_master_broadcast(ec_fsm_t *); -void ec_fsm_master_proc_states(ec_fsm_t *); -void ec_fsm_master_scan(ec_fsm_t *); -void ec_fsm_master_states(ec_fsm_t *); +void ec_fsm_master_read_states(ec_fsm_t *); void ec_fsm_master_validate_vendor(ec_fsm_t *); void ec_fsm_master_validate_product(ec_fsm_t *); -void ec_fsm_master_reconfigure(ec_fsm_t *); -void ec_fsm_master_address(ec_fsm_t *); -void ec_fsm_master_conf(ec_fsm_t *); -void ec_fsm_master_eeprom(ec_fsm_t *); - -void ec_fsm_slave_start_reading(ec_fsm_t *); -void ec_fsm_slave_read_status(ec_fsm_t *); -void ec_fsm_slave_read_base(ec_fsm_t *); -void ec_fsm_slave_read_dl(ec_fsm_t *); -void ec_fsm_slave_eeprom_size(ec_fsm_t *); -void ec_fsm_slave_fetch_eeprom(ec_fsm_t *); -void ec_fsm_slave_fetch_eeprom2(ec_fsm_t *); -void ec_fsm_slave_end(ec_fsm_t *); - -void ec_fsm_slave_conf(ec_fsm_t *); -void ec_fsm_slave_sync(ec_fsm_t *); -void ec_fsm_slave_preop(ec_fsm_t *); -void ec_fsm_slave_fmmu(ec_fsm_t *); -void ec_fsm_slave_saveop(ec_fsm_t *); -void ec_fsm_slave_op(ec_fsm_t *); -void ec_fsm_slave_op2(ec_fsm_t *); +void ec_fsm_master_rewrite_addresses(ec_fsm_t *); +void ec_fsm_master_configure_slave(ec_fsm_t *); +void ec_fsm_master_scan_slaves(ec_fsm_t *); +void ec_fsm_master_write_eeprom(ec_fsm_t *); + +void ec_fsm_slavescan_start(ec_fsm_t *); +void ec_fsm_slavescan_address(ec_fsm_t *); +void ec_fsm_slavescan_state(ec_fsm_t *); +void ec_fsm_slavescan_base(ec_fsm_t *); +void ec_fsm_slavescan_datalink(ec_fsm_t *); +void ec_fsm_slavescan_eeprom_size(ec_fsm_t *); +void ec_fsm_slavescan_eeprom_data(ec_fsm_t *); +void ec_fsm_slavescan_end(ec_fsm_t *); + +void ec_fsm_slaveconf_init(ec_fsm_t *); +void ec_fsm_slaveconf_sync(ec_fsm_t *); +void ec_fsm_slaveconf_preop(ec_fsm_t *); +void ec_fsm_slaveconf_fmmu(ec_fsm_t *); +void ec_fsm_slaveconf_saveop(ec_fsm_t *); +void ec_fsm_slaveconf_op(ec_fsm_t *); +void ec_fsm_slaveconf_end(ec_fsm_t *); void ec_fsm_sii_start_reading(ec_fsm_t *); void ec_fsm_sii_read_check(ec_fsm_t *); @@ -87,7 +85,7 @@ void ec_fsm_change_status(ec_fsm_t *); void ec_fsm_change_code(ec_fsm_t *); void ec_fsm_change_ack(ec_fsm_t *); -void ec_fsm_change_ack2(ec_fsm_t *); +void ec_fsm_change_check_ack(ec_fsm_t *); void ec_fsm_change_end(ec_fsm_t *); void ec_fsm_change_error(ec_fsm_t *); @@ -99,7 +97,7 @@ int ec_fsm_init(ec_fsm_t *fsm, /**< finite state machine */ ec_master_t *master /**< EtherCAT master */ - ) + ) { fsm->master = master; fsm->master_state = ec_fsm_master_start; @@ -107,9 +105,9 @@ fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN; fsm->master_validation = 0; - ec_command_init(&fsm->command); - if (ec_command_prealloc(&fsm->command, EC_MAX_DATA_SIZE)) { - EC_ERR("FSM failed to allocate FSM command.\n"); + ec_datagram_init(&fsm->datagram); + if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) { + EC_ERR("Failed to allocate FSM datagram.\n"); return -1; } @@ -124,7 +122,7 @@ void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_clear(&fsm->command); + ec_datagram_clear(&fsm->datagram); } /*****************************************************************************/ @@ -162,8 +160,8 @@ void ec_fsm_master_start(ec_fsm_t *fsm) { - ec_command_brd(&fsm->command, 0x0130, 2); - ec_master_queue_command(fsm->master, &fsm->command); + ec_datagram_brd(&fsm->datagram, 0x0130, 2); + ec_master_queue_datagram(fsm->master, &fsm->datagram); fsm->master_state = ec_fsm_master_broadcast; } @@ -176,12 +174,12 @@ void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; + ec_datagram_t *datagram = &fsm->datagram; unsigned int topology_change, states_change, i; ec_slave_t *slave; ec_master_t *master = fsm->master; - if (command->state != EC_CMD_RECEIVED) { + if (datagram->state != EC_CMD_RECEIVED) { if (!master->device->link_state) { fsm->master_slaves_responding = 0; list_for_each_entry(slave, &master->slaves, list) { @@ -193,12 +191,12 @@ return; } - topology_change = (command->working_counter != + topology_change = (datagram->working_counter != fsm->master_slaves_responding); - states_change = (EC_READ_U8(command->data) != fsm->master_slave_states); - - fsm->master_slave_states = EC_READ_U8(command->data); - fsm->master_slaves_responding = command->working_counter; + states_change = (EC_READ_U8(datagram->data) != fsm->master_slave_states); + + fsm->master_slave_states = EC_READ_U8(datagram->data); + fsm->master_slaves_responding = datagram->working_counter; if (topology_change) { EC_INFO("%i slave%s responding.\n", @@ -221,8 +219,8 @@ printk(".\n"); } - // topology change in free-run mode: clear all slaves and scan the bus - if (topology_change && master->mode == EC_MASTER_MODE_FREERUN) { + // topology change in idle mode: clear all slaves and scan the bus + if (topology_change && master->mode == EC_MASTER_MODE_IDLE) { EC_INFO("Scanning bus.\n"); ec_master_eoe_stop(master); @@ -264,17 +262,84 @@ // begin scanning of slaves fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); - fsm->slave_state = ec_fsm_slave_start_reading; - fsm->master_state = ec_fsm_master_scan; + fsm->slave_state = ec_fsm_slavescan_start; + fsm->master_state = ec_fsm_master_scan_slaves; fsm->master_state(fsm); // execute immediately return; } // fetch state from each slave fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); - ec_command_nprd(&fsm->command, fsm->slave->station_address, 0x0130, 2); - ec_master_queue_command(master, &fsm->command); - fsm->master_state = ec_fsm_master_states; + ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2); + ec_master_queue_datagram(master, &fsm->datagram); + fsm->master_state = ec_fsm_master_read_states; +} + +/*****************************************************************************/ + +/** + Master action: PROC_STATES. + Processes the slave states. +*/ + +void ec_fsm_master_action_process_states(ec_fsm_t *fsm + /**< finite state machine */ + ) +{ + ec_master_t *master = fsm->master; + ec_slave_t *slave; + + // check if any slaves are not in the state, they're supposed to be + list_for_each_entry(slave, &master->slaves, list) { + if (slave->error_flag || + !slave->online || + slave->requested_state == EC_SLAVE_STATE_UNKNOWN || + slave->current_state == slave->requested_state) continue; + + EC_INFO("Changing state of slave %i from ", slave->ring_position); + ec_print_states(slave->current_state); + printk(" to "); + ec_print_states(slave->requested_state); + printk(".\n"); + + fsm->slave = slave; + fsm->slave_state = ec_fsm_slaveconf_init; + fsm->change_new = EC_SLAVE_STATE_INIT; + fsm->change_state = ec_fsm_change_start; + fsm->master_state = ec_fsm_master_configure_slave; + fsm->master_state(fsm); // execute immediately + return; + } + + if (master->mode == EC_MASTER_MODE_IDLE) { + // nothing to configure. check for pending EEPROM write operations. + list_for_each_entry(slave, &master->slaves, list) { + if (!slave->new_eeprom_data) continue; + + if (!slave->online || slave->error_flag) { + kfree(slave->new_eeprom_data); + slave->new_eeprom_data = NULL; + EC_ERR("Discarding EEPROM data, slave %i not ready.\n", + slave->ring_position); + continue; + } + + // found pending EEPROM write operation. execute it! + EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position); + fsm->sii_offset = 0x0000; + memcpy(fsm->sii_value, slave->new_eeprom_data, 2); + fsm->sii_mode = 1; + fsm->sii_state = ec_fsm_sii_start_writing; + fsm->slave = slave; + fsm->master_state = ec_fsm_master_write_eeprom; + fsm->master_state(fsm); // execute immediately + return; + } + } + + // nothing to do. restart master state machine. + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately } /*****************************************************************************/ @@ -289,37 +354,38 @@ ec_master_t *master = fsm->master; ec_slave_t *slave = fsm->slave; - // have all states been read? - if (slave->list.next == &master->slaves) { - - // check, if a bus validation has to be done - if (fsm->master_validation) { - fsm->master_validation = 0; - list_for_each_entry(slave, &master->slaves, list) { - if (slave->online) continue; - - // At least one slave is offline. validate! - EC_INFO("Validating bus.\n"); - fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); - fsm->master_state = ec_fsm_master_validate_vendor; - fsm->sii_offset = 0x0008; // vendor ID - fsm->sii_mode = 0; - fsm->sii_state = ec_fsm_sii_start_reading; - fsm->sii_state(fsm); // execute immediately - return; - } + // is there another slave to query? + if (slave->list.next != &master->slaves) { + // process next slave + fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); + ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, + 0x0130, 2); + ec_master_queue_datagram(master, &fsm->datagram); + fsm->master_state = ec_fsm_master_read_states; + return; + } + + // all slave states read + + // check, if a bus validation has to be done + if (fsm->master_validation) { + fsm->master_validation = 0; + list_for_each_entry(slave, &master->slaves, list) { + if (slave->online) continue; + + // At least one slave is offline. validate! + EC_INFO("Validating bus.\n"); + fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); + fsm->master_state = ec_fsm_master_validate_vendor; + fsm->sii_offset = 0x0008; // vendor ID + fsm->sii_mode = 0; + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->sii_state(fsm); // execute immediately + return; } - - fsm->master_state = ec_fsm_master_proc_states; - fsm->master_state(fsm); // execute immediately - return; - } - - // process next slave - fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); - ec_command_nprd(&fsm->command, fsm->slave->station_address, 0x0130, 2); - ec_master_queue_command(master, &fsm->command); - fsm->master_state = ec_fsm_master_states; + } + + ec_fsm_master_action_process_states(fsm); } /*****************************************************************************/ @@ -329,20 +395,20 @@ Fetches the AL- and online state of a slave. */ -void ec_fsm_master_states(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_slave_t *slave = fsm->slave; - ec_command_t *command = &fsm->command; +void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = &fsm->datagram; uint8_t new_state; - if (command->state != EC_CMD_RECEIVED) { + if (datagram->state != EC_CMD_RECEIVED) { fsm->master_state = ec_fsm_master_start; fsm->master_state(fsm); // execute immediately return; } // did the slave not respond to its station address? - if (command->working_counter != 1) { + if (datagram->working_counter != 1) { if (slave->online) { slave->online = 0; EC_INFO("Slave %i: offline.\n", slave->ring_position); @@ -352,10 +418,10 @@ } // slave responded - new_state = EC_READ_U8(command->data); + new_state = EC_READ_U8(datagram->data); if (!slave->online) { // slave was offline before slave->online = 1; - slave->state_error = 0; + slave->error_flag = 0; // clear error flag slave->current_state = new_state; EC_INFO("Slave %i: online (", slave->ring_position); ec_print_states(new_state); @@ -376,62 +442,6 @@ /*****************************************************************************/ /** - Master state: PROC_STATES. - Processes the slave states. -*/ - -void ec_fsm_master_proc_states(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_master_t *master = fsm->master; - ec_slave_t *slave; - - // check if any slaves are not in the state, they're supposed to be - list_for_each_entry(slave, &master->slaves, list) { - if (slave->state_error || !slave->online || - slave->requested_state == EC_SLAVE_STATE_UNKNOWN || - slave->current_state == slave->requested_state) continue; - - EC_INFO("Changing state of slave %i from ", slave->ring_position); - ec_print_states(slave->current_state); - printk(" to "); - ec_print_states(slave->requested_state); - printk(".\n"); - - fsm->slave = slave; - fsm->slave_state = ec_fsm_slave_conf; - fsm->change_new = EC_SLAVE_STATE_INIT; - fsm->change_state = ec_fsm_change_start; - fsm->master_state = ec_fsm_master_conf; - fsm->master_state(fsm); // execute immediately - return; - } - - if (master->mode == EC_MASTER_MODE_FREERUN) { - // nothing to configure. check for pending EEPROM write operations. - list_for_each_entry(slave, &master->slaves, list) { - if (!slave->new_eeprom_data) continue; - - // found pending EEPROM write operation. execute it! - EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position); - fsm->sii_offset = 0x0000; - memcpy(fsm->sii_value, slave->new_eeprom_data, 2); - fsm->sii_mode = 1; - fsm->sii_state = ec_fsm_sii_start_writing; - fsm->slave = slave; - fsm->master_state = ec_fsm_master_eeprom; - fsm->master_state(fsm); // execute immediately - return; - } - } - - // nothing to do. restart master state machine. - fsm->master_state = ec_fsm_master_start; - fsm->master_state(fsm); // execute immediately -} - -/*****************************************************************************/ - -/** Master state: VALIDATE_VENDOR. Validates the vendor ID of a slave. */ @@ -443,6 +453,7 @@ fsm->sii_state(fsm); // execute SII state machine if (fsm->sii_state == ec_fsm_sii_error) { + fsm->slave->error_flag = 1; EC_ERR("Failed to validate vendor ID of slave %i.\n", slave->ring_position); fsm->master_state = ec_fsm_master_start; @@ -470,62 +481,14 @@ /*****************************************************************************/ /** - Master state: VALIDATE_PRODUCT. - Validates the product ID of a slave. -*/ - -void ec_fsm_master_validate_product(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_slave_t *slave = fsm->slave; - - fsm->sii_state(fsm); // execute SII state machine - - if (fsm->sii_state == ec_fsm_sii_error) { - EC_ERR("Failed to validate product code of slave %i.\n", - slave->ring_position); - fsm->master_state = ec_fsm_master_start; - fsm->master_state(fsm); // execute immediately - return; - } - - if (fsm->sii_state != ec_fsm_sii_end) return; - - if (EC_READ_U32(fsm->sii_value) != slave->sii_product_code) { - EC_ERR("Slave %i: invalid product code!\n", slave->ring_position); - EC_ERR("expected 0x%08X, got 0x%08X.\n", slave->sii_product_code, - EC_READ_U32(fsm->sii_value)); - fsm->master_state = ec_fsm_master_start; - fsm->master_state(fsm); // execute immediately - return; - } - - // have all states been validated? - if (slave->list.next == &fsm->master->slaves) { - fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list); - fsm->master_state = ec_fsm_master_reconfigure; - return; - } - - // validate next slave - fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); - fsm->master_state = ec_fsm_master_validate_vendor; - fsm->sii_offset = 0x0008; // vendor ID - fsm->sii_mode = 0; - fsm->sii_state = ec_fsm_sii_start_reading; - fsm->sii_state(fsm); // execute immediately -} - -/*****************************************************************************/ - -/** - Master state: RECONFIGURE. + Master action: ADDRESS. Looks for slave, that have lost their configuration and writes their station address, so that they can be reconfigured later. */ -void ec_fsm_master_reconfigure(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; +void ec_fsm_master_action_addresses(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; while (fsm->slave->online) { if (fsm->slave->list.next == &fsm->master->slaves) { // last slave? @@ -540,10 +503,60 @@ EC_INFO("Reinitializing slave %i.\n", fsm->slave->ring_position); // write station address - ec_command_apwr(command, fsm->slave->ring_position, 0x0010, 2); - EC_WRITE_U16(command->data, fsm->slave->station_address); - ec_master_queue_command(fsm->master, command); - fsm->master_state = ec_fsm_master_address; + ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2); + EC_WRITE_U16(datagram->data, fsm->slave->station_address); + ec_master_queue_datagram(fsm->master, datagram); + fsm->master_state = ec_fsm_master_rewrite_addresses; +} + +/*****************************************************************************/ + +/** + Master state: VALIDATE_PRODUCT. + Validates the product ID of a slave. +*/ + +void ec_fsm_master_validate_product(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_slave_t *slave = fsm->slave; + + fsm->sii_state(fsm); // execute SII state machine + + if (fsm->sii_state == ec_fsm_sii_error) { + fsm->slave->error_flag = 1; + EC_ERR("Failed to validate product code of slave %i.\n", + slave->ring_position); + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + if (fsm->sii_state != ec_fsm_sii_end) return; + + if (EC_READ_U32(fsm->sii_value) != slave->sii_product_code) { + EC_ERR("Slave %i: invalid product code!\n", slave->ring_position); + EC_ERR("expected 0x%08X, got 0x%08X.\n", slave->sii_product_code, + EC_READ_U32(fsm->sii_value)); + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + // have all states been validated? + if (slave->list.next == &fsm->master->slaves) { + fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list); + // start writing addresses to offline slaves + ec_fsm_master_action_addresses(fsm); + return; + } + + // validate next slave + fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); + fsm->master_state = ec_fsm_master_validate_vendor; + fsm->sii_offset = 0x0008; // vendor ID + fsm->sii_mode = 0; + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->sii_state(fsm); // execute immediately } /*****************************************************************************/ @@ -553,12 +566,14 @@ Checks, if the new station address has been written to the slave. */ -void ec_fsm_master_address(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_slave_t *slave = fsm->slave; - ec_command_t *command = &fsm->command; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { +void ec_fsm_master_rewrite_addresses(ec_fsm_t *fsm + /**< finite state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { EC_ERR("Failed to write station address on slave %i.\n", slave->ring_position); } @@ -571,8 +586,8 @@ // check next slave fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); - fsm->master_state = ec_fsm_master_reconfigure; - fsm->master_state(fsm); // execute immediately + // Write new station address to slave + ec_fsm_master_action_addresses(fsm); } /*****************************************************************************/ @@ -582,7 +597,7 @@ Executes the sub-statemachine for the scanning of a slave. */ -void ec_fsm_master_scan(ec_fsm_t *fsm /**< finite state machine */) +void ec_fsm_master_scan_slaves(ec_fsm_t *fsm /**< finite state machine */) { ec_master_t *master = fsm->master; ec_slave_t *slave = fsm->slave; @@ -592,7 +607,7 @@ fsm->slave_state(fsm); // execute slave state machine - if (fsm->slave_state != ec_fsm_slave_end) return; + if (fsm->slave_state != ec_fsm_slavescan_end) return; // have all slaves been fetched? if (slave->list.next == &master->slaves) { @@ -619,7 +634,7 @@ } if (!slave->type) { - EC_WARN("FSM: Unknown slave device (vendor 0x%08X," + EC_WARN("Unknown slave device (vendor 0x%08X," " code 0x%08X) at position %i.\n", slave->sii_vendor_id, slave->sii_product_code, slave->ring_position); @@ -636,7 +651,9 @@ } // determine initial state. - if ((slave->type && slave->type->special == EC_TYPE_BUS_COUPLER)) { + if ((slave->type && + (slave->type->special == EC_TYPE_BUS_COUPLER || + slave->type->special == EC_TYPE_INFRA))) { slave->requested_state = EC_SLAVE_STATE_OP; } else { @@ -645,7 +662,7 @@ else slave->requested_state = EC_SLAVE_STATE_INIT; } - slave->state_error = 0; + slave->error_flag = 0; // calculate coupler-based slave address slave->coupler_index = current_coupler_index; @@ -653,7 +670,7 @@ coupler_subindex++; } - if (master->mode == EC_MASTER_MODE_FREERUN) { + if (master->mode == EC_MASTER_MODE_IDLE) { // start EoE processing ec_master_eoe_start(master); } @@ -665,7 +682,7 @@ // process next slave fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); - fsm->slave_state = ec_fsm_slave_start_reading; + fsm->slave_state = ec_fsm_slavescan_start; fsm->slave_state(fsm); // execute immediately } @@ -676,12 +693,14 @@ Starts configuring a slave. */ -void ec_fsm_master_conf(ec_fsm_t *fsm /**< finite state machine */) +void ec_fsm_master_configure_slave(ec_fsm_t *fsm + /**< finite state machine */ + ) { fsm->slave_state(fsm); // execute slave's state machine - if (fsm->slave_state != ec_fsm_slave_end) return; - fsm->master_state = ec_fsm_master_proc_states; - fsm->master_state(fsm); // execute immediately + if (fsm->slave_state != ec_fsm_slaveconf_end) return; + + ec_fsm_master_action_process_states(fsm); } /*****************************************************************************/ @@ -690,13 +709,14 @@ Master state: EEPROM. */ -void ec_fsm_master_eeprom(ec_fsm_t *fsm /**< finite state machine */) +void ec_fsm_master_write_eeprom(ec_fsm_t *fsm /**< finite state machine */) { ec_slave_t *slave = fsm->slave; fsm->sii_state(fsm); // execute SII state machine if (fsm->sii_state == ec_fsm_sii_error) { + fsm->slave->error_flag = 1; EC_ERR("Failed to write EEPROM contents to slave %i.\n", slave->ring_position); kfree(slave->new_eeprom_data); @@ -722,13 +742,13 @@ slave->new_eeprom_data = NULL; // restart master state machine. - fsm->master_state = ec_fsm_master_start; + fsm->master_state = ec_fsm_master_start; // TODO: Evaluate new contents! fsm->master_state(fsm); // execute immediately return; } /****************************************************************************** - * slave state machine + * slave scan sub state machine *****************************************************************************/ /** @@ -737,126 +757,129 @@ slave, according to its ring position. */ -void ec_fsm_slave_start_reading(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; +void ec_fsm_slavescan_start(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; // write station address - ec_command_apwr(command, fsm->slave->ring_position, 0x0010, 2); - EC_WRITE_U16(command->data, fsm->slave->station_address); - ec_master_queue_command(fsm->master, command); - fsm->slave_state = ec_fsm_slave_read_status; -} - -/*****************************************************************************/ - -/** - Slave state: READ_STATUS. -*/ - -void ec_fsm_slave_read_status(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("FSM failed to write station address of slave %i.\n", + ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2); + EC_WRITE_U16(datagram->data, fsm->slave->station_address); + ec_master_queue_datagram(fsm->master, datagram); + fsm->slave_state = ec_fsm_slavescan_address; +} + +/*****************************************************************************/ + +/** + Slave state: ADDRESS. +*/ + +void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; + EC_ERR("Failed to write station address of slave %i.\n", fsm->slave->ring_position); - fsm->slave_state = ec_fsm_slave_end; - return; - } - - // read AL status - ec_command_nprd(command, fsm->slave->station_address, 0x0130, 2); - ec_master_queue_command(fsm->master, command); - fsm->slave_state = ec_fsm_slave_read_base; -} - -/*****************************************************************************/ - -/** - Slave state: READ_BASE. -*/ - -void ec_fsm_slave_read_base(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("FSM failed to read AL status of slave %i.\n", + return; + } + + // Read AL state + ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->master, datagram); + fsm->slave_state = ec_fsm_slavescan_state; +} + +/*****************************************************************************/ + +/** + Slave state: STATE. +*/ + +void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; + EC_ERR("Failed to read AL state of slave %i.\n", fsm->slave->ring_position); - fsm->slave_state = ec_fsm_slave_end; - return; - } - - slave->current_state = EC_READ_U8(command->data); + return; + } + + slave->current_state = EC_READ_U8(datagram->data); if (slave->current_state & EC_ACK) { - EC_WARN("Slave %i has status error bit set (0x%02X)!\n", + EC_WARN("Slave %i has state error bit set (0x%02X)!\n", slave->ring_position, slave->current_state); slave->current_state &= 0x0F; } // read base data - ec_command_nprd(command, fsm->slave->station_address, 0x0000, 6); - ec_master_queue_command(fsm->master, command); - fsm->slave_state = ec_fsm_slave_read_dl; -} - -/*****************************************************************************/ - -/** - Slave state: READ_DL. -*/ - -void ec_fsm_slave_read_dl(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("FSM failed to read base data of slave %i.\n", + ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6); + ec_master_queue_datagram(fsm->master, datagram); + fsm->slave_state = ec_fsm_slavescan_base; +} + +/*****************************************************************************/ + +/** + Slave state: BASE. +*/ + +void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; + EC_ERR("Failed to read base data of slave %i.\n", slave->ring_position); - fsm->slave_state = ec_fsm_slave_end; - return; - } - - slave->base_type = EC_READ_U8 (command->data); - slave->base_revision = EC_READ_U8 (command->data + 1); - slave->base_build = EC_READ_U16(command->data + 2); - slave->base_fmmu_count = EC_READ_U8 (command->data + 4); - slave->base_sync_count = EC_READ_U8 (command->data + 5); + return; + } + + slave->base_type = EC_READ_U8 (datagram->data); + slave->base_revision = EC_READ_U8 (datagram->data + 1); + slave->base_build = EC_READ_U16(datagram->data + 2); + slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); + slave->base_sync_count = EC_READ_U8 (datagram->data + 5); if (slave->base_fmmu_count > EC_MAX_FMMUS) slave->base_fmmu_count = EC_MAX_FMMUS; // read data link status - ec_command_nprd(command, slave->station_address, 0x0110, 2); - ec_master_queue_command(slave->master, command); - fsm->slave_state = ec_fsm_slave_eeprom_size; -} - -/*****************************************************************************/ - -/** - Slave state: EEPROM_SIZE. - Read the actual size of the EEPROM to allocate the EEPROM image. -*/ - -void ec_fsm_slave_eeprom_size(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; + ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2); + ec_master_queue_datagram(slave->master, datagram); + fsm->slave_state = ec_fsm_slavescan_datalink; +} + +/*****************************************************************************/ + +/** + Slave state: DATALINK. +*/ + +void ec_fsm_slavescan_datalink(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; ec_slave_t *slave = fsm->slave; uint16_t dl_status; unsigned int i; - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("FSM failed to read DL status of slave %i.\n", + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; + EC_ERR("Failed to read DL status of slave %i.\n", slave->ring_position); - fsm->slave_state = ec_fsm_slave_end; - return; - } - - dl_status = EC_READ_U16(command->data); + return; + } + + dl_status = EC_READ_U16(datagram->data); for (i = 0; i < 4; i++) { slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0; slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0; @@ -868,17 +891,17 @@ fsm->sii_offset = 0x0040; // first category header fsm->sii_mode = 1; fsm->sii_state = ec_fsm_sii_start_reading; - fsm->slave_state = ec_fsm_slave_fetch_eeprom; + fsm->slave_state = ec_fsm_slavescan_eeprom_size; fsm->slave_state(fsm); // execute state immediately } /*****************************************************************************/ /** - Slave state: FETCH_EEPROM. -*/ - -void ec_fsm_slave_fetch_eeprom(ec_fsm_t *fsm /**< finite state machine */) + Slave state: EEPROM_SIZE. +*/ + +void ec_fsm_slavescan_eeprom_size(ec_fsm_t *fsm /**< finite state machine */) { ec_slave_t *slave = fsm->slave; uint16_t cat_type, cat_size; @@ -887,7 +910,8 @@ fsm->sii_state(fsm); if (fsm->sii_state == ec_fsm_sii_error) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; EC_ERR("Failed to read EEPROM size of slave %i.\n", slave->ring_position); return; @@ -915,9 +939,10 @@ if (!(slave->eeprom_data = (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; EC_ERR("Failed to allocate EEPROM data on slave %i.\n", slave->ring_position); - fsm->slave_state = ec_fsm_slave_end; return; } @@ -926,17 +951,17 @@ fsm->sii_offset = 0x0000; fsm->sii_mode = 1; fsm->sii_state = ec_fsm_sii_start_reading; - fsm->slave_state = ec_fsm_slave_fetch_eeprom2; + fsm->slave_state = ec_fsm_slavescan_eeprom_data; fsm->slave_state(fsm); // execute state immediately } /*****************************************************************************/ /** - Slave state: FETCH_EEPROM2. -*/ - -void ec_fsm_slave_fetch_eeprom2(ec_fsm_t *fsm /**< finite state machine */) + Slave state: EEPROM_DATA. +*/ + +void ec_fsm_slavescan_eeprom_data(ec_fsm_t *fsm /**< finite state machine */) { ec_slave_t *slave = fsm->slave; uint16_t *cat_word, cat_type, cat_size; @@ -945,7 +970,8 @@ fsm->sii_state(fsm); if (fsm->sii_state == ec_fsm_sii_error) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; EC_ERR("Failed to fetch EEPROM contents of slave %i.\n", slave->ring_position); return; @@ -1033,26 +1059,45 @@ cat_word += cat_size + 2; } - end: - fsm->slave_state = ec_fsm_slave_end; -} - -/*****************************************************************************/ - -/** - Slave state: CONF. -*/ - -void ec_fsm_slave_conf(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_slave_t *slave = fsm->slave; - ec_master_t *master = fsm->master; - ec_command_t *command = &fsm->command; + fsm->slave_state = ec_fsm_slavescan_end; + +end: + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slavescan_end; +} + +/*****************************************************************************/ + +/** + Slave state: END. + End state of the slave state machine. +*/ + +void ec_fsm_slavescan_end(ec_fsm_t *fsm /**< finite state machine */) +{ +} + +/****************************************************************************** + * slave configuration sub state machine + *****************************************************************************/ + +/** + Slave state: INIT. +*/ + +void ec_fsm_slaveconf_init(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_slave_t *slave = fsm->slave; + ec_datagram_t *datagram = &fsm->datagram; + const ec_sync_t *sync; + ec_eeprom_sync_t *eeprom_sync, mbox_sync; + unsigned int j; fsm->change_state(fsm); // execute state change state machine if (fsm->change_state == ec_fsm_change_error) { - fsm->slave_state = ec_fsm_slave_end; + slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; return; } @@ -1060,7 +1105,7 @@ // slave is now in INIT if (slave->current_state == slave->requested_state) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave_state = ec_fsm_slaveconf_end; // successful return; } @@ -1071,150 +1116,114 @@ // check and reset CRC fault counters //ec_slave_check_crc(slave); - - if (!slave->base_fmmu_count) { // no fmmus - fsm->slave_state = ec_fsm_slave_sync; - fsm->slave_state(fsm); // execute immediately - return; - } - - // reset FMMUs - ec_command_npwr(command, slave->station_address, 0x0600, - EC_FMMU_SIZE * slave->base_fmmu_count); - memset(command->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); - ec_master_queue_command(master, command); - fsm->slave_state = ec_fsm_slave_sync; -} - -/*****************************************************************************/ - -/** - Slave state: SYNC. - Configure sync managers. -*/ - -void ec_fsm_slave_sync(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - unsigned int j; - const ec_sync_t *sync; - ec_eeprom_sync_t *eeprom_sync, mbox_sync; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("Failed to reset FMMUs of slave %i.\n", - slave->ring_position); - slave->state_error = 1; - fsm->slave_state = ec_fsm_slave_end; - return; - } + // TODO! if (!slave->base_sync_count) { // no sync managers - fsm->slave_state = ec_fsm_slave_preop; - fsm->slave_state(fsm); // execute immediately + fsm->slave_state = ec_fsm_slaveconf_preop; + fsm->change_new = EC_SLAVE_STATE_PREOP; + fsm->change_state = ec_fsm_change_start; + fsm->change_state(fsm); // execute immediately return; } // configure sync managers - ec_command_npwr(command, slave->station_address, 0x0800, - EC_SYNC_SIZE * slave->base_sync_count); - memset(command->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); + ec_datagram_npwr(datagram, slave->station_address, 0x0800, + EC_SYNC_SIZE * slave->base_sync_count); + memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); + + // does the slave supply sync manager configurations in its EEPROM? + if (!list_empty(&slave->eeprom_syncs)) { + list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { + if (eeprom_sync->index >= slave->base_sync_count) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; + EC_ERR("Invalid sync manager configuration found!"); + return; + } + ec_eeprom_sync_config(eeprom_sync, slave, + datagram->data + EC_SYNC_SIZE + * eeprom_sync->index); + } + } // known slave type, take type's SM information - if (slave->type) { + else if (slave->type) { for (j = 0; slave->type->sync_managers[j] && j < EC_MAX_SYNC; j++) { sync = slave->type->sync_managers[j]; - ec_sync_config(sync, slave, command->data + EC_SYNC_SIZE * j); + ec_sync_config(sync, slave, datagram->data + EC_SYNC_SIZE * j); } } // unknown type, but slave has mailbox else if (slave->sii_mailbox_protocols) { - // does it supply sync manager configurations in its EEPROM? - if (!list_empty(&slave->eeprom_syncs)) { - list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { - if (eeprom_sync->index >= slave->base_sync_count) { - EC_ERR("Invalid sync manager configuration found!"); - fsm->slave_state = ec_fsm_slave_end; - return; - } - ec_eeprom_sync_config(eeprom_sync, - command->data + EC_SYNC_SIZE - * eeprom_sync->index); - } - } - - // no sync manager information; guess mailbox settings - else { - mbox_sync.physical_start_address = - slave->sii_rx_mailbox_offset; - mbox_sync.length = slave->sii_rx_mailbox_size; - mbox_sync.control_register = 0x26; - mbox_sync.enable = 1; - ec_eeprom_sync_config(&mbox_sync, command->data); - - mbox_sync.physical_start_address = - slave->sii_tx_mailbox_offset; - mbox_sync.length = slave->sii_tx_mailbox_size; - mbox_sync.control_register = 0x22; - mbox_sync.enable = 1; - ec_eeprom_sync_config(&mbox_sync, - command->data + EC_SYNC_SIZE); - } + // guess mailbox settings + mbox_sync.physical_start_address = + slave->sii_rx_mailbox_offset; + mbox_sync.length = slave->sii_rx_mailbox_size; + mbox_sync.control_register = 0x26; + mbox_sync.enable = 1; + ec_eeprom_sync_config(&mbox_sync, slave, datagram->data); + + mbox_sync.physical_start_address = + slave->sii_tx_mailbox_offset; + mbox_sync.length = slave->sii_tx_mailbox_size; + mbox_sync.control_register = 0x22; + mbox_sync.enable = 1; + ec_eeprom_sync_config(&mbox_sync, slave, + datagram->data + EC_SYNC_SIZE); EC_INFO("Mailbox configured for unknown slave %i\n", slave->ring_position); } - ec_master_queue_command(fsm->master, command); - fsm->slave_state = ec_fsm_slave_preop; -} - -/*****************************************************************************/ - -/** - Slave state: PREOP. - Change slave state to PREOP. -*/ - -void ec_fsm_slave_preop(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { + ec_master_queue_datagram(fsm->master, datagram); + fsm->slave_state = ec_fsm_slaveconf_sync; +} + +/*****************************************************************************/ + +/** + Slave state: SYNC. +*/ + +void ec_fsm_slaveconf_sync(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; EC_ERR("Failed to set sync managers on slave %i.\n", slave->ring_position); - slave->state_error = 1; - fsm->slave_state = ec_fsm_slave_end; - return; - } - + return; + } + + fsm->slave_state = ec_fsm_slaveconf_preop; fsm->change_new = EC_SLAVE_STATE_PREOP; fsm->change_state = ec_fsm_change_start; - fsm->slave_state = ec_fsm_slave_fmmu; fsm->change_state(fsm); // execute immediately } /*****************************************************************************/ /** - Slave state: FMMU. - Configure FMMUs. -*/ - -void ec_fsm_slave_fmmu(ec_fsm_t *fsm /**< finite state machine */) + Slave state: PREOP. +*/ + +void ec_fsm_slaveconf_preop(ec_fsm_t *fsm /**< finite state machine */) { ec_slave_t *slave = fsm->slave; ec_master_t *master = fsm->master; - ec_command_t *command = &fsm->command; + ec_datagram_t *datagram = &fsm->datagram; unsigned int j; fsm->change_state(fsm); // execute state change state machine if (fsm->change_state == ec_fsm_change_error) { - fsm->slave_state = ec_fsm_slave_end; + slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; return; } @@ -1222,57 +1231,57 @@ // slave is now in PREOP if (slave->current_state == slave->requested_state) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave_state = ec_fsm_slaveconf_end; // successful return; } // stop activation here for slaves without type if (!slave->type) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave_state = ec_fsm_slaveconf_end; // successful return; } if (!slave->base_fmmu_count) { - fsm->slave_state = ec_fsm_slave_saveop; - fsm->slave_state(fsm); // execute immediately + fsm->slave_state = ec_fsm_slaveconf_saveop; + fsm->change_new = EC_SLAVE_STATE_SAVEOP; + fsm->change_state = ec_fsm_change_start; + fsm->change_state(fsm); // execute immediately return; } // configure FMMUs - ec_command_npwr(command, slave->station_address, - 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); - memset(command->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); + ec_datagram_npwr(datagram, slave->station_address, + 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); + memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); for (j = 0; j < slave->fmmu_count; j++) { ec_fmmu_config(&slave->fmmus[j], slave, - command->data + EC_FMMU_SIZE * j); - } - - ec_master_queue_command(master, command); - fsm->slave_state = ec_fsm_slave_saveop; -} - -/*****************************************************************************/ - -/** - Slave state: SAVEOP. - Set slave state to SAVEOP. -*/ - -void ec_fsm_slave_saveop(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - - if (fsm->slave->base_fmmu_count && (command->state != EC_CMD_RECEIVED || - command->working_counter != 1)) { - EC_ERR("FSM failed to set FMMUs on slave %i.\n", + datagram->data + EC_FMMU_SIZE * j); + } + + ec_master_queue_datagram(master, datagram); + fsm->slave_state = ec_fsm_slaveconf_fmmu; +} + +/*****************************************************************************/ + +/** + Slave state: FMMU. +*/ + +void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; + EC_ERR("Failed to set FMMUs on slave %i.\n", fsm->slave->ring_position); - fsm->slave->state_error = 1; - fsm->slave_state = ec_fsm_slave_end; return; } // set state to SAVEOP - fsm->slave_state = ec_fsm_slave_op; + fsm->slave_state = ec_fsm_slaveconf_saveop; fsm->change_new = EC_SLAVE_STATE_SAVEOP; fsm->change_state = ec_fsm_change_start; fsm->change_state(fsm); // execute immediately @@ -1281,16 +1290,16 @@ /*****************************************************************************/ /** - Slave state: OP. - Set slave state to OP. -*/ - -void ec_fsm_slave_op(ec_fsm_t *fsm /**< finite state machine */) + Slave state: SAVEOP. +*/ + +void ec_fsm_slaveconf_saveop(ec_fsm_t *fsm /**< finite state machine */) { fsm->change_state(fsm); // execute state change state machine if (fsm->change_state == ec_fsm_change_error) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; return; } @@ -1298,12 +1307,12 @@ // slave is now in SAVEOP if (fsm->slave->current_state == fsm->slave->requested_state) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave_state = ec_fsm_slaveconf_end; // successful return; } // set state to OP - fsm->slave_state = ec_fsm_slave_op2; + fsm->slave_state = ec_fsm_slaveconf_op; fsm->change_new = EC_SLAVE_STATE_OP; fsm->change_state = ec_fsm_change_start; fsm->change_state(fsm); // execute immediately @@ -1312,23 +1321,23 @@ /*****************************************************************************/ /** - Slave state: OP2 - Executes the change state machine, until the OP state is set. -*/ - -void ec_fsm_slave_op2(ec_fsm_t *fsm /**< finite state machine */) + Slave state: OP +*/ + +void ec_fsm_slaveconf_op(ec_fsm_t *fsm /**< finite state machine */) { fsm->change_state(fsm); // execute state change state machine if (fsm->change_state == ec_fsm_change_error) { - fsm->slave_state = ec_fsm_slave_end; + fsm->slave->error_flag = 1; + fsm->slave_state = ec_fsm_slaveconf_end; return; } if (fsm->change_state != ec_fsm_change_end) return; // slave is now in OP - fsm->slave_state = ec_fsm_slave_end; + fsm->slave_state = ec_fsm_slaveconf_end; // successful } /*****************************************************************************/ @@ -1338,12 +1347,12 @@ End state of the slave state machine. */ -void ec_fsm_slave_end(ec_fsm_t *fsm /**< finite state machine */) +void ec_fsm_slaveconf_end(ec_fsm_t *fsm /**< finite state machine */) { } /****************************************************************************** - * SII state machine + * SII sub state machine *****************************************************************************/ /** @@ -1353,20 +1362,20 @@ void ec_fsm_sii_start_reading(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; + ec_datagram_t *datagram = &fsm->datagram; // initiate read operation if (fsm->sii_mode) { - ec_command_npwr(command, fsm->slave->station_address, 0x502, 4); + ec_datagram_npwr(datagram, fsm->slave->station_address, 0x502, 4); } else { - ec_command_apwr(command, fsm->slave->ring_position, 0x502, 4); - } - - EC_WRITE_U8 (command->data, 0x00); // read-only access - EC_WRITE_U8 (command->data + 1, 0x01); // request read operation - EC_WRITE_U16(command->data + 2, fsm->sii_offset); - ec_master_queue_command(fsm->master, command); + ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4); + } + + EC_WRITE_U8 (datagram->data, 0x00); // read-only access + EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation + EC_WRITE_U16(datagram->data + 2, fsm->sii_offset); + ec_master_queue_datagram(fsm->master, datagram); fsm->sii_state = ec_fsm_sii_read_check; } @@ -1374,30 +1383,30 @@ /** SII state: READ_CHECK. - Checks, if the SII-read-command has been sent and issues a fetch command. + Checks, if the SII-read-datagram has been sent and issues a fetch datagram. */ void ec_fsm_sii_read_check(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("SII: Reception of read command failed.\n"); + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + EC_ERR("SII: Reception of read datagram failed.\n"); fsm->sii_state = ec_fsm_sii_error; return; } fsm->sii_start = get_cycles(); - // issue check/fetch command + // issue check/fetch datagram if (fsm->sii_mode) { - ec_command_nprd(command, fsm->slave->station_address, 0x502, 10); + ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); } else { - ec_command_aprd(command, fsm->slave->ring_position, 0x502, 10); - } - - ec_master_queue_command(fsm->master, command); + ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10); + } + + ec_master_queue_datagram(fsm->master, datagram); fsm->sii_state = ec_fsm_sii_read_fetch; } @@ -1405,55 +1414,55 @@ /** SII state: READ_FETCH. - Fetches the result of an SII-read command. + Fetches the result of an SII-read datagram. */ void ec_fsm_sii_read_fetch(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("SII: Reception of check/fetch command failed.\n"); + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + EC_ERR("SII: Reception of check/fetch datagram failed.\n"); fsm->sii_state = ec_fsm_sii_error; return; } // check "busy bit" - if (EC_READ_U8(command->data + 1) & 0x81) { + if (EC_READ_U8(datagram->data + 1) & 0x81) { // still busy... timeout? if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { EC_ERR("SII: Timeout.\n"); fsm->sii_state = ec_fsm_sii_error; #if 0 EC_DBG("SII busy: %02X %02X %02X %02X\n", - EC_READ_U8(command->data + 0), - EC_READ_U8(command->data + 1), - EC_READ_U8(command->data + 2), - EC_READ_U8(command->data + 3)); + EC_READ_U8(datagram->data + 0), + EC_READ_U8(datagram->data + 1), + EC_READ_U8(datagram->data + 2), + EC_READ_U8(datagram->data + 3)); #endif } - // issue check/fetch command again + // issue check/fetch datagram again if (fsm->sii_mode) { - ec_command_nprd(command, fsm->slave->station_address, 0x502, 10); + ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); } else { - ec_command_aprd(command, fsm->slave->ring_position, 0x502, 10); + ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10); } - ec_master_queue_command(fsm->master, command); + ec_master_queue_datagram(fsm->master, datagram); return; } #if 0 EC_DBG("SII rec: %02X %02X %02X %02X - %02X %02X %02X %02X\n", - EC_READ_U8(command->data + 0), EC_READ_U8(command->data + 1), - EC_READ_U8(command->data + 2), EC_READ_U8(command->data + 3), - EC_READ_U8(command->data + 6), EC_READ_U8(command->data + 7), - EC_READ_U8(command->data + 8), EC_READ_U8(command->data + 9)); + EC_READ_U8(datagram->data + 0), EC_READ_U8(datagram->data + 1), + EC_READ_U8(datagram->data + 2), EC_READ_U8(datagram->data + 3), + EC_READ_U8(datagram->data + 6), EC_READ_U8(datagram->data + 7), + EC_READ_U8(datagram->data + 8), EC_READ_U8(datagram->data + 9)); #endif // SII value received. - memcpy(fsm->sii_value, command->data + 6, 4); + memcpy(fsm->sii_value, datagram->data + 6, 4); fsm->sii_state = ec_fsm_sii_end; } @@ -1466,15 +1475,15 @@ void ec_fsm_sii_start_writing(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; + ec_datagram_t *datagram = &fsm->datagram; // initiate write operation - ec_command_npwr(command, fsm->slave->station_address, 0x502, 8); - EC_WRITE_U8 (command->data, 0x01); // enable write access - EC_WRITE_U8 (command->data + 1, 0x02); // request write operation - EC_WRITE_U32(command->data + 2, fsm->sii_offset); - memcpy(command->data + 6, fsm->sii_value, 2); - ec_master_queue_command(fsm->master, command); + ec_datagram_npwr(datagram, fsm->slave->station_address, 0x502, 8); + EC_WRITE_U8 (datagram->data, 0x01); // enable write access + EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation + EC_WRITE_U32(datagram->data + 2, fsm->sii_offset); + memcpy(datagram->data + 6, fsm->sii_value, 2); + ec_master_queue_datagram(fsm->master, datagram); fsm->sii_state = ec_fsm_sii_write_check; } @@ -1486,19 +1495,19 @@ void ec_fsm_sii_write_check(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("SII: Reception of write command failed.\n"); + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + EC_ERR("SII: Reception of write datagram failed.\n"); fsm->sii_state = ec_fsm_sii_error; return; } fsm->sii_start = get_cycles(); - // issue check/fetch command - ec_command_nprd(command, fsm->slave->station_address, 0x502, 2); - ec_master_queue_command(fsm->master, command); + // issue check/fetch datagram + ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2); + ec_master_queue_datagram(fsm->master, datagram); fsm->sii_state = ec_fsm_sii_write_check2; } @@ -1510,25 +1519,25 @@ void ec_fsm_sii_write_check2(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("SII: Reception of write check command failed.\n"); + ec_datagram_t *datagram = &fsm->datagram; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { + EC_ERR("SII: Reception of write check datagram failed.\n"); fsm->sii_state = ec_fsm_sii_error; return; } - if (EC_READ_U8(command->data + 1) & 0x82) { + if (EC_READ_U8(datagram->data + 1) & 0x82) { // still busy... timeout? if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { EC_ERR("SII: Write timeout.\n"); fsm->sii_state = ec_fsm_sii_error; } - // issue check/fetch command again - ec_master_queue_command(fsm->master, command); - } - else if (EC_READ_U8(command->data + 1) & 0x40) { + // issue check/fetch datagram again + ec_master_queue_datagram(fsm->master, datagram); + } + else if (EC_READ_U8(datagram->data + 1) & 0x40) { EC_ERR("SII: Write operation failed!\n"); fsm->sii_state = ec_fsm_sii_error; } @@ -1560,7 +1569,7 @@ } /****************************************************************************** - * state change state machine + * state change sub state machine *****************************************************************************/ /** @@ -1569,13 +1578,13 @@ void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; + ec_datagram_t *datagram = &fsm->datagram; ec_slave_t *slave = fsm->slave; // write new state to slave - ec_command_npwr(command, slave->station_address, 0x0120, 2); - EC_WRITE_U16(command->data, fsm->change_new); - ec_master_queue_command(fsm->master, command); + ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); + EC_WRITE_U16(datagram->data, fsm->change_new); + ec_master_queue_datagram(fsm->master, datagram); fsm->change_state = ec_fsm_change_check; } @@ -1587,53 +1596,50 @@ void ec_fsm_change_check(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED) { - EC_ERR("Failed to send state command to slave %i!\n", + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_CMD_RECEIVED) { + fsm->change_state = ec_fsm_change_error; + EC_ERR("Failed to send state datagram to slave %i!\n", fsm->slave->ring_position); - slave->state_error = 1; + return; + } + + if (datagram->working_counter != 1) { fsm->change_state = ec_fsm_change_error; - return; - } - - if (command->working_counter != 1) { EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" " respond.\n", fsm->change_new, fsm->slave->ring_position); - slave->state_error = 1; + return; + } + + fsm->change_start = get_cycles(); + + // read AL status from slave + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->master, datagram); + fsm->change_state = ec_fsm_change_status; +} + +/*****************************************************************************/ + +/** + Change state: STATUS. +*/ + +void ec_fsm_change_status(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { fsm->change_state = ec_fsm_change_error; - return; - } - - fsm->change_start = get_cycles(); - - // read AL status from slave - ec_command_nprd(command, slave->station_address, 0x0130, 2); - ec_master_queue_command(fsm->master, command); - fsm->change_state = ec_fsm_change_status; -} - -/*****************************************************************************/ - -/** - Change state: STATUS. -*/ - -void ec_fsm_change_status(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { EC_ERR("Failed to check state 0x%02X on slave %i.\n", fsm->change_new, slave->ring_position); - slave->state_error = 1; - fsm->change_state = ec_fsm_change_error; - return; - } - - slave->current_state = EC_READ_U8(command->data); + return; + } + + slave->current_state = EC_READ_U8(datagram->data); if (slave->current_state == fsm->change_new) { // state has been set successfully @@ -1648,15 +1654,14 @@ " (code 0x%02X)!\n", fsm->change_new, slave->ring_position, slave->current_state); // fetch AL status error code - ec_command_nprd(command, slave->station_address, 0x0134, 2); - ec_master_queue_command(fsm->master, command); + ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); + ec_master_queue_datagram(fsm->master, datagram); fsm->change_state = ec_fsm_change_code; return; } if (get_cycles() - fsm->change_start >= (cycles_t) 10 * cpu_khz) { // timeout while checking - slave->state_error = 1; fsm->change_state = ec_fsm_change_error; EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", fsm->change_new, slave->ring_position); @@ -1664,8 +1669,8 @@ } // still old state: check again - ec_command_nprd(command, slave->station_address, 0x0130, 2); - ec_master_queue_command(fsm->master, command); + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->master, datagram); } /*****************************************************************************/ @@ -1687,10 +1692,24 @@ {0x0019, "No valid outputs"}, {0x001A, "Synchronisation error"}, {0x001B, "Sync manager watchdog"}, + {0x001C, "Invalid sync manager types"}, + {0x001D, "Invalid output configuration"}, + {0x001E, "Invalid input configuration"}, + {0x001F, "Invalid watchdog configuration"}, {0x0020, "Slave needs cold start"}, {0x0021, "Slave needs INIT"}, {0x0022, "Slave needs PREOP"}, {0x0023, "Slave needs SAVEOP"}, + {0x0030, "Invalid DC SYNCH configuration"}, + {0x0031, "Invalid DC latch configuration"}, + {0x0032, "PLL error"}, + {0x0033, "Invalid DC IO error"}, + {0x0034, "Invalid DC timeout error"}, + {0x0042, "MBOX EOE"}, + {0x0043, "MBOX COE"}, + {0x0044, "MBOX FOE"}, + {0x0045, "MBOX SOE"}, + {0x004F, "MBOX VOE"}, {} }; @@ -1702,19 +1721,18 @@ void ec_fsm_change_code(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; + ec_datagram_t *datagram = &fsm->datagram; ec_slave_t *slave = fsm->slave; uint32_t code; const ec_code_msg_t *al_msg; - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("Reception of AL status code command failed.\n"); - slave->state_error = 1; + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { fsm->change_state = ec_fsm_change_error; - return; - } - - if ((code = EC_READ_U16(command->data))) { + EC_ERR("Reception of AL status code datagram failed.\n"); + return; + } + + if ((code = EC_READ_U16(datagram->data))) { for (al_msg = al_status_messages; al_msg->code; al_msg++) { if (al_msg->code != code) continue; EC_ERR("AL status message 0x%04X: \"%s\".\n", @@ -1726,9 +1744,9 @@ } // acknowledge "old" slave state - ec_command_npwr(command, slave->station_address, 0x0120, 2); - EC_WRITE_U16(command->data, slave->current_state); - ec_master_queue_command(fsm->master, command); + ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); + EC_WRITE_U16(datagram->data, slave->current_state); + ec_master_queue_datagram(fsm->master, datagram); fsm->change_state = ec_fsm_change_ack; } @@ -1740,55 +1758,62 @@ void ec_fsm_change_ack(ec_fsm_t *fsm /**< finite state machine */) { - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("Reception of state ack command failed.\n"); - slave->state_error = 1; + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { fsm->change_state = ec_fsm_change_error; - return; - } + EC_ERR("Reception of state ack datagram failed.\n"); + return; + } + + fsm->change_start = get_cycles(); // read new AL status - ec_command_nprd(command, slave->station_address, 0x0130, 2); - ec_master_queue_command(fsm->master, command); - fsm->change_state = ec_fsm_change_ack2; -} - -/*****************************************************************************/ - -/** - Change state: ACK. - Acknowledge 2. -*/ - -void ec_fsm_change_ack2(ec_fsm_t *fsm /**< finite state machine */) -{ - ec_command_t *command = &fsm->command; - ec_slave_t *slave = fsm->slave; - - if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { - EC_ERR("Reception of state ack check command failed.\n"); - slave->state_error = 1; + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->master, datagram); + fsm->change_state = ec_fsm_change_check_ack; +} + +/*****************************************************************************/ + +/** + Change state: CHECK ACK. +*/ + +void ec_fsm_change_check_ack(ec_fsm_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = &fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_slave_state_t ack_state; + + if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { fsm->change_state = ec_fsm_change_error; - return; - } - - slave->current_state = EC_READ_U8(command->data); - - if (slave->current_state == fsm->change_new) { + EC_ERR("Reception of state ack check datagram failed.\n"); + return; + } + + ack_state = EC_READ_U8(datagram->data); + + if (ack_state == slave->current_state) { + fsm->change_state = ec_fsm_change_error; EC_INFO("Acknowleged state 0x%02X on slave %i.\n", slave->current_state, slave->ring_position); - slave->state_error = 1; + return; + } + + if (get_cycles() - fsm->change_start >= (cycles_t) 100 * cpu_khz) { + // timeout while checking + slave->current_state = EC_SLAVE_STATE_UNKNOWN; fsm->change_state = ec_fsm_change_error; - return; - } - - EC_WARN("Failed to acknowledge state 0x%02X on slave %i" - " - Timeout!\n", fsm->change_new, slave->ring_position); - slave->state_error = 1; - fsm->change_state = ec_fsm_change_error; + EC_ERR("Timeout while acknowleging state 0x%02X on slave %i.\n", + fsm->change_new, slave->ring_position); + return; + } + + // reread new AL status + ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); + ec_master_queue_datagram(fsm->master, datagram); } /*****************************************************************************/ diff -r 05622513f627 -r 9dc190591c0f master/fsm.h --- a/master/fsm.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/fsm.h Wed Aug 02 12:25:25 2006 +0000 @@ -42,7 +42,7 @@ #define __EC_STATES__ #include "../include/ecrt.h" -#include "command.h" +#include "datagram.h" #include "slave.h" /*****************************************************************************/ @@ -57,7 +57,7 @@ { ec_master_t *master; /**< master the FSM runs on */ ec_slave_t *slave; /**< slave the FSM runs on */ - ec_command_t command; /**< command used in the state machine */ + ec_datagram_t datagram; /**< datagram used in the state machine */ void (*master_state)(ec_fsm_t *); /**< master state function */ unsigned int master_slaves_responding; /**< number of responding slaves */ diff -r 05622513f627 -r 9dc190591c0f master/globals.h --- a/master/globals.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/globals.h Wed Aug 02 12:25:25 2006 +0000 @@ -72,11 +72,11 @@ /** size of an EtherCAT frame header */ #define EC_FRAME_HEADER_SIZE 2 -/** size of an EtherCAT command header */ -#define EC_COMMAND_HEADER_SIZE 10 +/** size of an EtherCAT datagram header */ +#define EC_DATAGRAM_HEADER_SIZE 10 -/** size of an EtherCAT command footer */ -#define EC_COMMAND_FOOTER_SIZE 2 +/** size of an EtherCAT datagram footer */ +#define EC_DATAGRAM_FOOTER_SIZE 2 /** size of a sync manager configuration page */ #define EC_SYNC_SIZE 8 @@ -84,9 +84,9 @@ /** size of an FMMU configuration page */ #define EC_FMMU_SIZE 16 -/** resulting maximum data size of a single command in a frame */ +/** resulting maximum data size of a single datagram in a frame */ #define EC_MAX_DATA_SIZE (ETH_DATA_LEN - EC_FRAME_HEADER_SIZE \ - - EC_COMMAND_HEADER_SIZE - EC_COMMAND_FOOTER_SIZE) + - EC_DATAGRAM_HEADER_SIZE - EC_DATAGRAM_FOOTER_SIZE) /*****************************************************************************/ @@ -178,7 +178,7 @@ /** Code - Message pair. - Some EtherCAT commands support reading a status code to display a certain + Some EtherCAT datagrams support reading a status code to display a certain message. This type allows to map a code to a message string. */ diff -r 05622513f627 -r 9dc190591c0f master/mailbox.c --- a/master/mailbox.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/mailbox.c Wed Aug 02 12:25:25 2006 +0000 @@ -42,18 +42,18 @@ #include #include "mailbox.h" -#include "command.h" +#include "datagram.h" #include "master.h" /*****************************************************************************/ /** - Prepares a mailbox-send command. - \return pointer to mailbox command data + Prepares a mailbox-send datagram. + \return pointer to mailbox datagram data */ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */ - ec_command_t *command, /**< command */ + ec_datagram_t *datagram, /**< datagram */ uint8_t type, /**< mailbox protocol */ size_t size /**< size of the data */ ) @@ -72,32 +72,32 @@ return NULL; } - if (ec_command_npwr(command, slave->station_address, - slave->sii_rx_mailbox_offset, - slave->sii_rx_mailbox_size)) - return NULL; - - EC_WRITE_U16(command->data, size); // mailbox service data length - EC_WRITE_U16(command->data + 2, slave->station_address); // station address - EC_WRITE_U8 (command->data + 4, 0x00); // channel & priority - EC_WRITE_U8 (command->data + 5, type); // underlying protocol type - - return command->data + 6; -} - -/*****************************************************************************/ - -/** - Prepares a command for checking the mailbox state. + if (ec_datagram_npwr(datagram, slave->station_address, + slave->sii_rx_mailbox_offset, + slave->sii_rx_mailbox_size)) + return NULL; + + EC_WRITE_U16(datagram->data, size); // mailbox service data length + EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr. + EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority + EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type + + return datagram->data + 6; +} + +/*****************************************************************************/ + +/** + Prepares a datagram for checking the mailbox state. \return 0 in case of success, else < 0 */ int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */ - ec_command_t *command /**< command */ + ec_datagram_t *datagram /**< datagram */ ) { // FIXME: second sync manager? - if (ec_command_nprd(command, slave->station_address, 0x808, 8)) + if (ec_datagram_nprd(datagram, slave->station_address, 0x808, 8)) return -1; return 0; @@ -106,29 +106,29 @@ /*****************************************************************************/ /** - Processes a mailbox state checking command. + Processes a mailbox state checking datagram. \return 0 in case of success, else < 0 */ -int ec_slave_mbox_check(const ec_command_t *command /**< command */) -{ - return EC_READ_U8(command->data + 5) & 8 ? 1 : 0; -} - -/*****************************************************************************/ - -/** - Prepares a command to fetch mailbox data. +int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */) +{ + return EC_READ_U8(datagram->data + 5) & 8 ? 1 : 0; +} + +/*****************************************************************************/ + +/** + Prepares a datagram to fetch mailbox data. \return 0 in case of success, else < 0 */ int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */ - ec_command_t *command /**< command */ + ec_datagram_t *datagram /**< datagram */ ) { - if (ec_command_nprd(command, slave->station_address, - slave->sii_tx_mailbox_offset, - slave->sii_tx_mailbox_size)) return -1; + if (ec_datagram_nprd(datagram, slave->station_address, + slave->sii_tx_mailbox_offset, + slave->sii_tx_mailbox_size)) return -1; return 0; } @@ -140,64 +140,64 @@ */ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ - ec_command_t *command, /**< command */ + ec_datagram_t *datagram, /**< datagram */ uint8_t type, /**< expected mailbox protocol */ size_t *size /**< size of the received data */ ) { size_t data_size; - if ((EC_READ_U8(command->data + 5) & 0x0F) != type) { + if ((EC_READ_U8(datagram->data + 5) & 0x0F) != type) { EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at" - " slave %i!\n", EC_READ_U8(command->data + 5), type, - slave->ring_position); - return NULL; - } - - if ((data_size = EC_READ_U16(command->data)) > + " slave %i!\n", EC_READ_U8(datagram->data + 5), type, + slave->ring_position); + return NULL; + } + + if ((data_size = EC_READ_U16(datagram->data)) > slave->sii_tx_mailbox_size - 6) { EC_ERR("Currupt mailbox response detected!\n"); return NULL; } *size = data_size; - return command->data + 6; -} - -/*****************************************************************************/ - -/** - Sends a mailbox command and waits for its reception. + return datagram->data + 6; +} + +/*****************************************************************************/ + +/** + Sends a mailbox datagram and waits for its reception. \return pointer to the received data */ uint8_t *ec_slave_mbox_simple_io(const ec_slave_t *slave, /**< slave */ - ec_command_t *command, /**< command */ + ec_datagram_t *datagram, /**< datagram */ size_t *size /**< size of the received data */ ) { uint8_t type; - type = EC_READ_U8(command->data + 5); - - if (unlikely(ec_master_simple_io(slave->master, command))) { + type = EC_READ_U8(datagram->data + 5); + + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position); return NULL; } - return ec_slave_mbox_simple_receive(slave, command, type, size); -} - -/*****************************************************************************/ - -/** - Waits for the reception of a mailbox command. + return ec_slave_mbox_simple_receive(slave, datagram, type, size); +} + +/*****************************************************************************/ + +/** + Waits for the reception of a mailbox datagram. \return pointer to the received data */ uint8_t *ec_slave_mbox_simple_receive(const ec_slave_t *slave, /**< slave */ - ec_command_t *command, /**< command */ + ec_datagram_t *datagram, /**< datagram */ uint8_t type, /**< expected protocol */ size_t *size /**< received data size */ ) @@ -209,8 +209,8 @@ while (1) { - if (ec_slave_mbox_prepare_check(slave, command)) return NULL; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_slave_mbox_prepare_check(slave, datagram)) return NULL; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position); return NULL; @@ -218,7 +218,7 @@ end = get_cycles(); - if (ec_slave_mbox_check(command)) + if (ec_slave_mbox_check(datagram)) break; // proceed with receiving data if ((end - start) >= timeout) { @@ -230,18 +230,18 @@ udelay(100); } - if (ec_slave_mbox_prepare_fetch(slave, command)) return NULL; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_slave_mbox_prepare_fetch(slave, datagram)) return NULL; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Mailbox receiving failed on slave %i!\n", slave->ring_position); return NULL; } if (unlikely(slave->master->debug_level) > 1) - EC_DBG("Mailbox receive took %ius.\n", ((u32) (end - start) * 1000 - / cpu_khz)); - - return ec_slave_mbox_fetch(slave, command, type, size); -} - -/*****************************************************************************/ + EC_DBG("Mailbox receive took %ius.\n", + ((unsigned int) (end - start) * 1000 / cpu_khz)); + + return ec_slave_mbox_fetch(slave, datagram, type, size); +} + +/*****************************************************************************/ diff -r 05622513f627 -r 9dc190591c0f master/mailbox.h --- a/master/mailbox.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/mailbox.h Wed Aug 02 12:25:25 2006 +0000 @@ -45,16 +45,17 @@ /*****************************************************************************/ -uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_command_t *, +uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_datagram_t *, uint8_t, size_t); -int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_command_t *); -int ec_slave_mbox_check(const ec_command_t *); -int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_command_t *); -uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, ec_command_t *, +int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_datagram_t *); +int ec_slave_mbox_check(const ec_datagram_t *); +int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_datagram_t *); +uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, ec_datagram_t *, uint8_t, size_t *); -uint8_t *ec_slave_mbox_simple_io(const ec_slave_t *, ec_command_t *, size_t *); -uint8_t *ec_slave_mbox_simple_receive(const ec_slave_t *, ec_command_t *, +uint8_t *ec_slave_mbox_simple_io(const ec_slave_t *, ec_datagram_t *, + size_t *); +uint8_t *ec_slave_mbox_simple_receive(const ec_slave_t *, ec_datagram_t *, uint8_t, size_t *); /*****************************************************************************/ diff -r 05622513f627 -r 9dc190591c0f master/master.c --- a/master/master.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/master.c Wed Aug 02 12:25:25 2006 +0000 @@ -50,12 +50,12 @@ #include "slave.h" #include "types.h" #include "device.h" -#include "command.h" +#include "datagram.h" #include "ethernet.h" /*****************************************************************************/ -void ec_master_freerun(void *); +void ec_master_idle_run(void *); void ec_master_eoe_run(unsigned long); ssize_t ec_show_master_attribute(struct kobject *, struct attribute *, char *); ssize_t ec_store_master_attribute(struct kobject *, struct attribute *, @@ -68,11 +68,13 @@ EC_SYSFS_READ_ATTR(slave_count); EC_SYSFS_READ_ATTR(mode); EC_SYSFS_READ_WRITE_ATTR(eeprom_write_enable); +EC_SYSFS_READ_WRITE_ATTR(debug_level); static struct attribute *ec_def_attrs[] = { &attr_slave_count, &attr_mode, &attr_eeprom_write_enable, + &attr_debug_level, NULL, }; @@ -98,7 +100,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ unsigned int index, /**< master index */ - unsigned int eoe_devices /**< number of EoE devices */ + unsigned int eoeif_count /**< number of EoE interfaces */ ) { ec_eoe_t *eoe, *next_eoe; @@ -110,11 +112,11 @@ master->device = NULL; master->reserved = 0; INIT_LIST_HEAD(&master->slaves); - INIT_LIST_HEAD(&master->command_queue); + INIT_LIST_HEAD(&master->datagram_queue); INIT_LIST_HEAD(&master->domains); INIT_LIST_HEAD(&master->eoe_handlers); - ec_command_init(&master->simple_command); - INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master); + ec_datagram_init(&master->simple_datagram); + INIT_WORK(&master->idle_work, ec_master_idle_run, (void *) master); init_timer(&master->eoe_timer); master->eoe_timer.function = ec_master_eoe_run; master->eoe_timer.data = (unsigned long) master; @@ -128,7 +130,7 @@ } // create EoE handlers - for (i = 0; i < eoe_devices; i++) { + for (i = 0; i < eoeif_count; i++) { if (!(eoe = (ec_eoe_t *) kmalloc(sizeof(ec_eoe_t), GFP_KERNEL))) { EC_ERR("Failed to allocate EoE-Object.\n"); goto out_clear_eoe; @@ -171,7 +173,7 @@ /** Master destructor. - Removes all pending commands, clears the slave list, clears all domains + Removes all pending datagrams, clears the slave list, clears all domains and frees the device. */ @@ -184,7 +186,7 @@ ec_master_reset(master); ec_fsm_clear(&master->fsm); - ec_command_clear(&master->simple_command); + ec_datagram_clear(&master->simple_datagram); destroy_workqueue(master->workqueue); // clear EoE objects @@ -212,17 +214,18 @@ void ec_master_reset(ec_master_t *master /**< EtherCAT master */) { - ec_command_t *command, *next_c; + ec_datagram_t *datagram, *next_c; ec_domain_t *domain, *next_d; ec_master_eoe_stop(master); - ec_master_freerun_stop(master); + ec_master_idle_stop(master); ec_master_clear_slaves(master); - // empty command queue - list_for_each_entry_safe(command, next_c, &master->command_queue, queue) { - list_del_init(&command->queue); - command->state = EC_CMD_ERROR; + // empty datagram queue + list_for_each_entry_safe(datagram, next_c, + &master->datagram_queue, queue) { + list_del_init(&datagram->queue); + datagram->state = EC_CMD_ERROR; } // clear domains @@ -232,7 +235,7 @@ kobject_put(&domain->kobj); } - master->command_index = 0; + master->datagram_index = 0; master->debug_level = 0; master->timeout = 500; // 500us @@ -242,7 +245,7 @@ master->stats.unmatched = 0; master->stats.t_last = 0; - master->mode = EC_MASTER_MODE_IDLE; + master->mode = EC_MASTER_MODE_ORPHANED; master->request_cb = NULL; master->release_cb = NULL; @@ -274,101 +277,101 @@ /*****************************************************************************/ /** - Places a command in the command queue. -*/ - -void ec_master_queue_command(ec_master_t *master, /**< EtherCAT master */ - ec_command_t *command /**< command */ - ) -{ - ec_command_t *queued_command; - - // check, if the command is already queued - list_for_each_entry(queued_command, &master->command_queue, queue) { - if (queued_command == command) { - command->state = EC_CMD_QUEUED; + Places a datagram in the datagram queue. +*/ + +void ec_master_queue_datagram(ec_master_t *master, /**< EtherCAT master */ + ec_datagram_t *datagram /**< datagram */ + ) +{ + ec_datagram_t *queued_datagram; + + // check, if the datagram is already queued + list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { + if (queued_datagram == datagram) { + datagram->state = EC_CMD_QUEUED; if (unlikely(master->debug_level)) - EC_WARN("command already queued.\n"); + EC_WARN("datagram already queued.\n"); return; } } - list_add_tail(&command->queue, &master->command_queue); - command->state = EC_CMD_QUEUED; -} - -/*****************************************************************************/ - -/** - Sends the commands in the queue. + list_add_tail(&datagram->queue, &master->datagram_queue); + datagram->state = EC_CMD_QUEUED; +} + +/*****************************************************************************/ + +/** + Sends the datagrams in the queue. \return 0 in case of success, else < 0 */ -void ec_master_send_commands(ec_master_t *master /**< EtherCAT master */) -{ - ec_command_t *command; - size_t command_size; +void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) +{ + ec_datagram_t *datagram; + size_t datagram_size; uint8_t *frame_data, *cur_data; void *follows_word; cycles_t t_start, t_end; - unsigned int frame_count, more_commands_waiting; + unsigned int frame_count, more_datagrams_waiting; frame_count = 0; t_start = get_cycles(); - if (unlikely(master->debug_level > 0)) - EC_DBG("ec_master_send_commands\n"); + if (unlikely(master->debug_level > 1)) + EC_DBG("ec_master_send_datagrams\n"); do { // fetch pointer to transmit socket buffer frame_data = ec_device_tx_data(master->device); cur_data = frame_data + EC_FRAME_HEADER_SIZE; follows_word = NULL; - more_commands_waiting = 0; - - // fill current frame with commands - list_for_each_entry(command, &master->command_queue, queue) { - if (command->state != EC_CMD_QUEUED) continue; - - // does the current command fit in the frame? - command_size = EC_COMMAND_HEADER_SIZE + command->data_size - + EC_COMMAND_FOOTER_SIZE; - if (cur_data - frame_data + command_size > ETH_DATA_LEN) { - more_commands_waiting = 1; + more_datagrams_waiting = 0; + + // fill current frame with datagrams + list_for_each_entry(datagram, &master->datagram_queue, queue) { + if (datagram->state != EC_CMD_QUEUED) continue; + + // does the current datagram fit in the frame? + datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size + + EC_DATAGRAM_FOOTER_SIZE; + if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { + more_datagrams_waiting = 1; break; } - command->state = EC_CMD_SENT; - command->t_sent = t_start; - command->index = master->command_index++; - - if (unlikely(master->debug_level > 0)) - EC_DBG("adding command 0x%02X\n", command->index); - - // set "command following" flag in previous frame + datagram->state = EC_CMD_SENT; + datagram->t_sent = t_start; + datagram->index = master->datagram_index++; + + if (unlikely(master->debug_level > 1)) + EC_DBG("adding datagram 0x%02X\n", datagram->index); + + // set "datagram following" flag in previous frame if (follows_word) EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000); - // EtherCAT command header - EC_WRITE_U8 (cur_data, command->type); - EC_WRITE_U8 (cur_data + 1, command->index); - EC_WRITE_U32(cur_data + 2, command->address.logical); - EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF); + // EtherCAT datagram header + EC_WRITE_U8 (cur_data, datagram->type); + EC_WRITE_U8 (cur_data + 1, datagram->index); + EC_WRITE_U32(cur_data + 2, datagram->address.logical); + EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF); EC_WRITE_U16(cur_data + 8, 0x0000); follows_word = cur_data + 6; - cur_data += EC_COMMAND_HEADER_SIZE; - - // EtherCAT command data - memcpy(cur_data, command->data, command->data_size); - cur_data += command->data_size; - - // EtherCAT command footer + cur_data += EC_DATAGRAM_HEADER_SIZE; + + // EtherCAT datagram data + memcpy(cur_data, datagram->data, datagram->data_size); + cur_data += datagram->data_size; + + // EtherCAT datagram footer EC_WRITE_U16(cur_data, 0x0000); // reset working counter - cur_data += EC_COMMAND_FOOTER_SIZE; + cur_data += EC_DATAGRAM_FOOTER_SIZE; } if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) { - if (unlikely(master->debug_level > 0)) + if (unlikely(master->debug_level > 1)) EC_DBG("nothing to send.\n"); break; } @@ -381,19 +384,19 @@ while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN) EC_WRITE_U8(cur_data++, 0x00); - if (unlikely(master->debug_level > 0)) + if (unlikely(master->debug_level > 1)) EC_DBG("frame size: %i\n", cur_data - frame_data); // send frame ec_device_send(master->device, cur_data - frame_data); frame_count++; } - while (more_commands_waiting); - - if (unlikely(master->debug_level > 0)) { + while (more_datagrams_waiting); + + if (unlikely(master->debug_level > 1)) { t_end = get_cycles(); - EC_DBG("ec_master_send_commands sent %i frames in %ius.\n", - frame_count, (u32) (t_end - t_start) * 1000 / cpu_khz); + EC_DBG("ec_master_send_datagrams sent %i frames in %ius.\n", + frame_count, (unsigned int) (t_end - t_start) * 1000 / cpu_khz); } } @@ -411,10 +414,10 @@ ) { size_t frame_size, data_size; - uint8_t command_type, command_index; + uint8_t datagram_type, datagram_index; unsigned int cmd_follows, matched; const uint8_t *cur_data; - ec_command_t *command; + ec_datagram_t *datagram; if (unlikely(size < EC_FRAME_HEADER_SIZE)) { master->stats.corrupted++; @@ -436,64 +439,64 @@ cmd_follows = 1; while (cmd_follows) { - // process command header - command_type = EC_READ_U8 (cur_data); - command_index = EC_READ_U8 (cur_data + 1); - data_size = EC_READ_U16(cur_data + 6) & 0x07FF; - cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000; - cur_data += EC_COMMAND_HEADER_SIZE; + // process datagram header + datagram_type = EC_READ_U8 (cur_data); + datagram_index = EC_READ_U8 (cur_data + 1); + data_size = EC_READ_U16(cur_data + 6) & 0x07FF; + cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000; + cur_data += EC_DATAGRAM_HEADER_SIZE; if (unlikely(cur_data - frame_data - + data_size + EC_COMMAND_FOOTER_SIZE > size)) { + + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) { master->stats.corrupted++; ec_master_output_stats(master); return; } - // search for matching command in the queue + // search for matching datagram in the queue matched = 0; - list_for_each_entry(command, &master->command_queue, queue) { - if (command->state == EC_CMD_SENT - && command->type == command_type - && command->index == command_index - && command->data_size == data_size) { + list_for_each_entry(datagram, &master->datagram_queue, queue) { + if (datagram->state == EC_CMD_SENT + && datagram->type == datagram_type + && datagram->index == datagram_index + && datagram->data_size == data_size) { matched = 1; break; } } - // no matching command was found + // no matching datagram was found if (!matched) { master->stats.unmatched++; ec_master_output_stats(master); - cur_data += data_size + EC_COMMAND_FOOTER_SIZE; + cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE; continue; } - // copy received data in the command memory - memcpy(command->data, cur_data, data_size); + // copy received data in the datagram memory + memcpy(datagram->data, cur_data, data_size); cur_data += data_size; - // set the command's working counter - command->working_counter = EC_READ_U16(cur_data); - cur_data += EC_COMMAND_FOOTER_SIZE; - - // dequeue the received command - command->state = EC_CMD_RECEIVED; - list_del_init(&command->queue); - } -} - -/*****************************************************************************/ - -/** - Sends a single command and waits for its reception. - If the slave doesn't respond, the command is sent again. + // set the datagram's working counter + datagram->working_counter = EC_READ_U16(cur_data); + cur_data += EC_DATAGRAM_FOOTER_SIZE; + + // dequeue the received datagram + datagram->state = EC_CMD_RECEIVED; + list_del_init(&datagram->queue); + } +} + +/*****************************************************************************/ + +/** + Sends a single datagram and waits for its reception. + If the slave doesn't respond, the datagram is sent again. \return 0 in case of success, else < 0 */ int ec_master_simple_io(ec_master_t *master, /**< EtherCAT master */ - ec_command_t *command /**< command */ + ec_datagram_t *datagram /**< datagram */ ) { unsigned int response_tries_left; @@ -502,19 +505,19 @@ while (1) { - ec_master_queue_command(master, command); + ec_master_queue_datagram(master, datagram); ecrt_master_sync_io(master); - if (command->state == EC_CMD_RECEIVED) { - if (likely(command->working_counter)) + if (datagram->state == EC_CMD_RECEIVED) { + if (likely(datagram->working_counter)) return 0; } - else if (command->state == EC_CMD_TIMEOUT) { + else if (datagram->state == EC_CMD_TIMEOUT) { EC_ERR("Simple-IO TIMEOUT!\n"); return -1; } - else if (command->state == EC_CMD_ERROR) { - EC_ERR("Simple-IO command error!\n"); + else if (datagram->state == EC_CMD_ERROR) { + EC_ERR("Simple-IO datagram error!\n"); return -1; } @@ -540,7 +543,7 @@ { ec_slave_t *slave; ec_slave_ident_t *ident; - ec_command_t *command; + ec_datagram_t *datagram; unsigned int i; uint16_t coupler_index, coupler_subindex; uint16_t reverse_coupler_index, current_coupler_index; @@ -550,12 +553,12 @@ return -1; } - command = &master->simple_command; + datagram = &master->simple_datagram; // determine number of slaves on bus - if (ec_command_brd(command, 0x0000, 4)) return -1; - if (unlikely(ec_master_simple_io(master, command))) return -1; - master->slave_count = command->working_counter; + if (ec_datagram_brd(datagram, 0x0000, 4)) return -1; + if (unlikely(ec_master_simple_io(master, datagram))) return -1; + master->slave_count = datagram->working_counter; EC_INFO("Found %i slaves on bus.\n", master->slave_count); if (!master->slave_count) return 0; @@ -588,10 +591,10 @@ list_for_each_entry(slave, &master->slaves, list) { // write station address - if (ec_command_apwr(command, slave->ring_position, 0x0010, + if (ec_datagram_apwr(datagram, slave->ring_position, 0x0010, sizeof(uint16_t))) goto out_free; - EC_WRITE_U16(command->data, slave->station_address); - if (unlikely(ec_master_simple_io(master, command))) { + EC_WRITE_U16(datagram->data, slave->station_address); + if (unlikely(ec_master_simple_io(master, datagram))) { EC_ERR("Writing station address failed on slave %i!\n", slave->ring_position); goto out_free; @@ -650,7 +653,7 @@ if (unlikely((u32) (t_now - master->stats.t_last) / cpu_khz > 1000)) { if (master->stats.timeouts) { - EC_WARN("%i commands TIMED OUT!\n", master->stats.timeouts); + EC_WARN("%i datagrams TIMED OUT!\n", master->stats.timeouts); master->stats.timeouts = 0; } if (master->stats.delayed) { @@ -662,7 +665,7 @@ master->stats.corrupted = 0; } if (master->stats.unmatched) { - EC_WARN("%i command(s) UNMATCHED!\n", master->stats.unmatched); + EC_WARN("%i datagram(s) UNMATCHED!\n", master->stats.unmatched); master->stats.unmatched = 0; } master->stats.t_last = t_now; @@ -672,41 +675,43 @@ /*****************************************************************************/ /** - Starts the Free-Run mode. -*/ - -void ec_master_freerun_start(ec_master_t *master /**< EtherCAT master */) -{ - if (master->mode == EC_MASTER_MODE_FREERUN) return; + Starts the Idle mode. +*/ + +void ec_master_idle_start(ec_master_t *master /**< EtherCAT master */) +{ + if (master->mode == EC_MASTER_MODE_IDLE) return; if (master->mode == EC_MASTER_MODE_RUNNING) { - EC_ERR("ec_master_freerun_start: Master already running!\n"); + EC_ERR("ec_master_idle_start: Master already running!\n"); return; } - EC_INFO("Starting Free-Run mode.\n"); - - master->mode = EC_MASTER_MODE_FREERUN; + EC_INFO("Starting Idle mode.\n"); + + master->mode = EC_MASTER_MODE_IDLE; ec_fsm_reset(&master->fsm); - queue_delayed_work(master->workqueue, &master->freerun_work, 1); -} - -/*****************************************************************************/ - -/** - Stops the Free-Run mode. -*/ - -void ec_master_freerun_stop(ec_master_t *master /**< EtherCAT master */) -{ - if (master->mode != EC_MASTER_MODE_FREERUN) return; + queue_delayed_work(master->workqueue, &master->idle_work, 1); +} + +/*****************************************************************************/ + +/** + Stops the Idle mode. +*/ + +void ec_master_idle_stop(ec_master_t *master /**< EtherCAT master */) +{ + if (master->mode != EC_MASTER_MODE_IDLE) return; ec_master_eoe_stop(master); - EC_INFO("Stopping Free-Run mode.\n"); - master->mode = EC_MASTER_MODE_IDLE; - - if (!cancel_delayed_work(&master->freerun_work)) { + EC_INFO("Stopping Idle mode.\n"); + master->mode = EC_MASTER_MODE_ORPHANED; // this is important for the + // IDLE work function to not + // queue itself again + + if (!cancel_delayed_work(&master->idle_work)) { flush_workqueue(master->workqueue); } @@ -716,10 +721,10 @@ /*****************************************************************************/ /** - Free-Run mode function. -*/ - -void ec_master_freerun(void *data /**< master pointer */) + Idle mode function. +*/ + +void ec_master_idle_run(void *data /**< master pointer */) { ec_master_t *master = (ec_master_t *) data; @@ -736,8 +741,8 @@ // release master lock spin_unlock_bh(&master->internal_lock); - if (master->mode == EC_MASTER_MODE_FREERUN) - queue_delayed_work(master->workqueue, &master->freerun_work, 1); + if (master->mode == EC_MASTER_MODE_IDLE) + queue_delayed_work(master->workqueue, &master->idle_work, 1); } /*****************************************************************************/ @@ -771,11 +776,24 @@ */ void ec_eeprom_sync_config(const ec_eeprom_sync_t *sync, /**< sync manager */ + const ec_slave_t *slave, /**< EtherCAT slave */ uint8_t *data /**> configuration memory */ ) { + size_t sync_size; + + sync_size = ec_slave_calc_eeprom_sync_size(slave, sync); + + if (slave->master->debug_level) { + EC_INFO("Slave %i, sync manager %i:\n", slave->ring_position, + sync->index); + EC_INFO(" Address: 0x%04X\n", sync->physical_start_address); + EC_INFO(" Size: %i\n", sync_size); + EC_INFO(" Control: 0x%02X\n", sync->control_register); + } + EC_WRITE_U16(data, sync->physical_start_address); - EC_WRITE_U16(data + 2, sync->length); + EC_WRITE_U16(data + 2, sync_size); EC_WRITE_U8 (data + 4, sync->control_register); EC_WRITE_U8 (data + 5, 0x00); // status byte (read only) EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable @@ -827,14 +845,20 @@ } else if (attr == &attr_mode) { switch (master->mode) { + case EC_MASTER_MODE_ORPHANED: + return sprintf(buffer, "ORPHANED\n"); case EC_MASTER_MODE_IDLE: return sprintf(buffer, "IDLE\n"); - case EC_MASTER_MODE_FREERUN: - return sprintf(buffer, "FREERUN\n"); case EC_MASTER_MODE_RUNNING: return sprintf(buffer, "RUNNING\n"); } } + else if (attr == &attr_debug_level) { + return sprintf(buffer, "%i\n", master->debug_level); + } + else if (attr == &attr_eeprom_write_enable) { + return sprintf(buffer, "%i\n", master->eeprom_write_enable); + } return 0; } @@ -873,6 +897,24 @@ EC_INFO("Slave EEPROM writing disabled.\n"); } } + else if (attr == &attr_debug_level) { + if (!strcmp(buffer, "0\n")) { + master->debug_level = 0; + } + else if (!strcmp(buffer, "1\n")) { + master->debug_level = 1; + } + else if (!strcmp(buffer, "2\n")) { + master->debug_level = 2; + } + else { + EC_ERR("Invalid debug level value!\n"); + return -EINVAL; + } + + EC_INFO("Master debug level set to %i.\n", master->debug_level); + return size; + } return -EINVAL; } @@ -914,14 +956,14 @@ else { slave->requested_state = EC_SLAVE_STATE_INIT; } - slave->state_error = 0; + slave->error_flag = 0; break; } if (!found) { EC_WARN("No EoE handler for slave %i!\n", slave->ring_position); slave->requested_state = EC_SLAVE_STATE_INIT; - slave->state_error = 0; + slave->error_flag = 0; } } @@ -959,7 +1001,7 @@ list_for_each_entry(eoe, &master->eoe_handlers, list) { if (eoe->slave) { eoe->slave->requested_state = EC_SLAVE_STATE_INIT; - eoe->slave->state_error = 0; + eoe->slave->error_flag = 0; eoe->slave = NULL; } } @@ -989,7 +1031,7 @@ if (master->request_cb(master->cb_data)) goto queue_timer; } - else if (master->mode == EC_MASTER_MODE_FREERUN) { + else if (master->mode == EC_MASTER_MODE_IDLE) { spin_lock(&master->internal_lock); } else @@ -1006,7 +1048,7 @@ if (master->mode == EC_MASTER_MODE_RUNNING) { master->release_cb(master->cb_data); } - else if (master->mode == EC_MASTER_MODE_FREERUN) { + else if (master->mode == EC_MASTER_MODE_IDLE) { spin_unlock(&master->internal_lock); } @@ -1075,7 +1117,7 @@ { unsigned int j; ec_slave_t *slave; - ec_command_t *command; + ec_datagram_t *datagram; const ec_sync_t *sync; const ec_slave_type_t *type; const ec_fmmu_t *fmmu; @@ -1083,7 +1125,7 @@ ec_domain_t *domain; ec_eeprom_sync_t *eeprom_sync, mbox_sync; - command = &master->simple_command; + datagram = &master->simple_datagram; // allocate all domains domain_offset = 0; @@ -1112,11 +1154,12 @@ // reset FMMUs if (slave->base_fmmu_count) { - if (ec_command_npwr(command, slave->station_address, 0x0600, - EC_FMMU_SIZE * slave->base_fmmu_count)) + if (ec_datagram_npwr(datagram, slave->station_address, 0x0600, + EC_FMMU_SIZE * slave->base_fmmu_count)) return -1; - memset(command->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); - if (unlikely(ec_master_simple_io(master, command))) { + memset(datagram->data, 0x00, + EC_FMMU_SIZE * slave->base_fmmu_count); + if (unlikely(ec_master_simple_io(master, datagram))) { EC_ERR("Resetting FMMUs failed on slave %i!\n", slave->ring_position); return -1; @@ -1125,11 +1168,12 @@ // reset sync managers if (slave->base_sync_count) { - if (ec_command_npwr(command, slave->station_address, 0x0800, + if (ec_datagram_npwr(datagram, slave->station_address, 0x0800, EC_SYNC_SIZE * slave->base_sync_count)) return -1; - memset(command->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); - if (unlikely(ec_master_simple_io(master, command))) { + memset(datagram->data, 0x00, + EC_SYNC_SIZE * slave->base_sync_count); + if (unlikely(ec_master_simple_io(master, datagram))) { EC_ERR("Resetting sync managers failed on slave %i!\n", slave->ring_position); return -1; @@ -1137,68 +1181,67 @@ } // configure sync managers - if (type) { // known slave type, take type's SM information + + // does the slave provide sync manager information? + if (!list_empty(&slave->eeprom_syncs)) { + list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { + if (ec_datagram_npwr(datagram, slave->station_address, + 0x800 + eeprom_sync->index * + EC_SYNC_SIZE, + EC_SYNC_SIZE)) return -1; + ec_eeprom_sync_config(eeprom_sync, slave, datagram->data); + if (unlikely(ec_master_simple_io(master, datagram))) { + EC_ERR("Setting sync manager %i failed on slave %i!\n", + eeprom_sync->index, slave->ring_position); + return -1; + } + } + } + + else if (type) { // known slave type, take type's SM information for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) { sync = type->sync_managers[j]; - if (ec_command_npwr(command, slave->station_address, - 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE)) + if (ec_datagram_npwr(datagram, slave->station_address, + 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE)) return -1; - ec_sync_config(sync, slave, command->data); - if (unlikely(ec_master_simple_io(master, command))) { + ec_sync_config(sync, slave, datagram->data); + if (unlikely(ec_master_simple_io(master, datagram))) { EC_ERR("Setting sync manager %i failed on slave %i!\n", j, slave->ring_position); return -1; } } } - else if (slave->sii_mailbox_protocols) { // unknown type, but mailbox - // does the device supply SM configurations in its EEPROM? - if (!list_empty(&slave->eeprom_syncs)) { - list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { - EC_INFO("Sync manager %i...\n", eeprom_sync->index); - if (ec_command_npwr(command, slave->station_address, - 0x800 + eeprom_sync->index * - EC_SYNC_SIZE, - EC_SYNC_SIZE)) return -1; - ec_eeprom_sync_config(eeprom_sync, command->data); - if (unlikely(ec_master_simple_io(master, command))) { - EC_ERR("Setting sync manager %i failed on slave %i!\n", - eeprom_sync->index, slave->ring_position); - return -1; - } - } + + // no sync manager information; guess mailbox settings + else if (slave->sii_mailbox_protocols) { + mbox_sync.physical_start_address = + slave->sii_rx_mailbox_offset; + mbox_sync.length = slave->sii_rx_mailbox_size; + mbox_sync.control_register = 0x26; + mbox_sync.enable = 1; + if (ec_datagram_npwr(datagram, slave->station_address, + 0x800,EC_SYNC_SIZE)) return -1; + ec_eeprom_sync_config(&mbox_sync, slave, datagram->data); + if (unlikely(ec_master_simple_io(master, datagram))) { + EC_ERR("Setting sync manager 0 failed on slave %i!\n", + slave->ring_position); + return -1; } - else { // no sync manager information; guess mailbox settings - mbox_sync.physical_start_address = - slave->sii_rx_mailbox_offset; - mbox_sync.length = slave->sii_rx_mailbox_size; - mbox_sync.control_register = 0x26; - mbox_sync.enable = 1; - if (ec_command_npwr(command, slave->station_address, - 0x800,EC_SYNC_SIZE)) return -1; - ec_eeprom_sync_config(&mbox_sync, command->data); - if (unlikely(ec_master_simple_io(master, command))) { - EC_ERR("Setting sync manager 0 failed on slave %i!\n", - slave->ring_position); - return -1; - } - - mbox_sync.physical_start_address = - slave->sii_tx_mailbox_offset; - mbox_sync.length = slave->sii_tx_mailbox_size; - mbox_sync.control_register = 0x22; - mbox_sync.enable = 1; - if (ec_command_npwr(command, slave->station_address, - 0x808, EC_SYNC_SIZE)) return -1; - ec_eeprom_sync_config(&mbox_sync, command->data); - if (unlikely(ec_master_simple_io(master, command))) { + + mbox_sync.physical_start_address = + slave->sii_tx_mailbox_offset; + mbox_sync.length = slave->sii_tx_mailbox_size; + mbox_sync.control_register = 0x22; + mbox_sync.enable = 1; + if (ec_datagram_npwr(datagram, slave->station_address, + 0x808, EC_SYNC_SIZE)) return -1; + ec_eeprom_sync_config(&mbox_sync, slave, datagram->data); + if (unlikely(ec_master_simple_io(master, datagram))) { EC_ERR("Setting sync manager 1 failed on slave %i!\n", slave->ring_position); return -1; - } - } - EC_INFO("Mailbox configured for unknown slave %i\n", - slave->ring_position); + } } // change state to PREOP @@ -1218,11 +1261,11 @@ // configure FMMUs for (j = 0; j < slave->fmmu_count; j++) { fmmu = &slave->fmmus[j]; - if (ec_command_npwr(command, slave->station_address, - 0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE)) + if (ec_datagram_npwr(datagram, slave->station_address, + 0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE)) return -1; - ec_fmmu_config(fmmu, slave, command->data); - if (unlikely(ec_master_simple_io(master, command))) { + ec_fmmu_config(fmmu, slave, datagram->data); + if (unlikely(ec_master_simple_io(master, datagram))) { EC_ERR("Setting FMMU %i failed on slave %i!\n", j, slave->ring_position); return -1; @@ -1288,17 +1331,17 @@ /*****************************************************************************/ /** - Sends queued commands and waits for their reception. + Sends queued datagrams and waits for their reception. \ingroup RealtimeInterface */ void ecrt_master_sync_io(ec_master_t *master /**< EtherCAT master */) { - ec_command_t *command, *n; - unsigned int commands_sent; + ec_datagram_t *datagram, *n; + unsigned int datagrams_sent; cycles_t t_start, t_end, t_timeout; - // send commands + // send datagrams ecrt_master_async_send(master); t_start = get_cycles(); // measure io time @@ -1310,23 +1353,23 @@ t_end = get_cycles(); // take current time if (t_end - t_start >= t_timeout) break; // timeout! - commands_sent = 0; - list_for_each_entry_safe(command, n, &master->command_queue, queue) { - if (command->state == EC_CMD_RECEIVED) - list_del_init(&command->queue); - else if (command->state == EC_CMD_SENT) - commands_sent++; - } - - if (!commands_sent) break; - } - - // timeout; dequeue all commands - list_for_each_entry_safe(command, n, &master->command_queue, queue) { - switch (command->state) { + datagrams_sent = 0; + list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { + if (datagram->state == EC_CMD_RECEIVED) + list_del_init(&datagram->queue); + else if (datagram->state == EC_CMD_SENT) + datagrams_sent++; + } + + if (!datagrams_sent) break; + } + + // timeout; dequeue all datagrams + list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { + switch (datagram->state) { case EC_CMD_SENT: case EC_CMD_QUEUED: - command->state = EC_CMD_TIMEOUT; + datagram->state = EC_CMD_TIMEOUT; master->stats.timeouts++; ec_master_output_stats(master); break; @@ -1337,26 +1380,26 @@ default: break; } - list_del_init(&command->queue); - } -} - -/*****************************************************************************/ - -/** - Asynchronous sending of commands. + list_del_init(&datagram->queue); + } +} + +/*****************************************************************************/ + +/** + Asynchronous sending of datagrams. \ingroup RealtimeInterface */ void ecrt_master_async_send(ec_master_t *master /**< EtherCAT master */) { - ec_command_t *command, *n; + ec_datagram_t *datagram, *n; if (unlikely(!master->device->link_state)) { - // link is down, no command can be sent - list_for_each_entry_safe(command, n, &master->command_queue, queue) { - command->state = EC_CMD_ERROR; - list_del_init(&command->queue); + // link is down, no datagram can be sent + list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { + datagram->state = EC_CMD_ERROR; + list_del_init(&datagram->queue); } // query link state @@ -1365,19 +1408,19 @@ } // send frames - ec_master_send_commands(master); -} - -/*****************************************************************************/ - -/** - Asynchronous receiving of commands. + ec_master_send_datagrams(master); +} + +/*****************************************************************************/ + +/** + Asynchronous receiving of datagrams. \ingroup RealtimeInterface */ void ecrt_master_async_receive(ec_master_t *master /**< EtherCAT master */) { - ec_command_t *command, *next; + ec_datagram_t *datagram, *next; cycles_t t_received, t_timeout; ec_device_call_isr(master->device); @@ -1385,18 +1428,19 @@ t_received = get_cycles(); t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000); - // dequeue all received commands - list_for_each_entry_safe(command, next, &master->command_queue, queue) - if (command->state == EC_CMD_RECEIVED) list_del_init(&command->queue); - - // dequeue all commands that timed out - list_for_each_entry_safe(command, next, &master->command_queue, queue) { - switch (command->state) { + // dequeue all received datagrams + list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) + if (datagram->state == EC_CMD_RECEIVED) + list_del_init(&datagram->queue); + + // dequeue all datagrams that timed out + list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { + switch (datagram->state) { case EC_CMD_SENT: case EC_CMD_QUEUED: - if (t_received - command->t_sent > t_timeout) { - list_del_init(&command->queue); - command->state = EC_CMD_TIMEOUT; + if (t_received - datagram->t_sent > t_timeout) { + list_del_init(&datagram->queue); + datagram->state = EC_CMD_TIMEOUT; master->stats.timeouts++; ec_master_output_stats(master); } @@ -1411,7 +1455,7 @@ /** Prepares synchronous IO. - Queues all domain commands and sends them. Then waits a certain time, so + Queues all domain datagrams and sends them. Then waits a certain time, so that ecrt_master_sasync_receive() can be called securely. \ingroup RealtimeInterface */ @@ -1421,7 +1465,7 @@ ec_domain_t *domain; cycles_t t_start, t_end, t_timeout; - // queue commands of all domains + // queue datagrams of all domains list_for_each_entry(domain, &master->domains, list) ecrt_domain_queue(domain); diff -r 05622513f627 -r 9dc190591c0f master/master.h --- a/master/master.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/master.h Wed Aug 02 12:25:25 2006 +0000 @@ -57,8 +57,8 @@ typedef enum { + EC_MASTER_MODE_ORPHANED, EC_MASTER_MODE_IDLE, - EC_MASTER_MODE_FREERUN, EC_MASTER_MODE_RUNNING } ec_master_mode_t; @@ -71,10 +71,10 @@ typedef struct { - unsigned int timeouts; /**< command timeouts */ - unsigned int delayed; /**< delayed commands */ + unsigned int timeouts; /**< datagram timeouts */ + unsigned int delayed; /**< delayed datagrams */ unsigned int corrupted; /**< corrupted frames */ - unsigned int unmatched; /**< unmatched commands */ + unsigned int unmatched; /**< unmatched datagrams */ cycles_t t_last; /**< time of last output */ } ec_stats_t; @@ -99,26 +99,27 @@ ec_device_t *device; /**< EtherCAT device */ - struct list_head command_queue; /**< command queue */ - uint8_t command_index; /**< current command index */ + struct list_head datagram_queue; /**< datagram queue */ + uint8_t datagram_index; /**< current datagram index */ struct list_head domains; /**< list of domains */ - ec_command_t simple_command; /**< command structure for initialization */ + ec_datagram_t simple_datagram; /**< datagram structure for + initialization */ unsigned int timeout; /**< timeout in synchronous IO */ int debug_level; /**< master debug level */ ec_stats_t stats; /**< cyclic statistics */ struct workqueue_struct *workqueue; /**< master workqueue */ - struct work_struct freerun_work; /**< free run work object */ + struct work_struct idle_work; /**< free run work object */ ec_fsm_t fsm; /**< master state machine */ ec_master_mode_t mode; /**< master mode */ struct timer_list eoe_timer; /**< EoE timer object */ unsigned int eoe_running; /**< non-zero, if EoE processing is active. */ struct list_head eoe_handlers; /**< Ethernet-over-EtherCAT handlers */ - spinlock_t internal_lock; /**< spinlock used in freerun mode */ + spinlock_t internal_lock; /**< spinlock used in idle mode */ int (*request_cb)(void *); /**< lock request callback */ void (*release_cb)(void *); /**< lock release callback */ void *cb_data; /**< data parameter of locking callbacks */ @@ -134,8 +135,8 @@ void ec_master_reset(ec_master_t *); // free run -void ec_master_freerun_start(ec_master_t *); -void ec_master_freerun_stop(ec_master_t *); +void ec_master_idle_start(ec_master_t *); +void ec_master_idle_stop(ec_master_t *); // EoE void ec_master_eoe_start(ec_master_t *); @@ -143,8 +144,8 @@ // IO void ec_master_receive(ec_master_t *, const uint8_t *, size_t); -void ec_master_queue_command(ec_master_t *, ec_command_t *); -int ec_master_simple_io(ec_master_t *, ec_command_t *); +void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *); +int ec_master_simple_io(ec_master_t *, ec_datagram_t *); // slave management int ec_master_bus_scan(ec_master_t *); @@ -152,7 +153,8 @@ // misc. void ec_master_clear_slaves(ec_master_t *); void ec_sync_config(const ec_sync_t *, const ec_slave_t *, uint8_t *); -void ec_eeprom_sync_config(const ec_eeprom_sync_t *, uint8_t *); +void ec_eeprom_sync_config(const ec_eeprom_sync_t *, const ec_slave_t *, + uint8_t *); void ec_fmmu_config(const ec_fmmu_t *, const ec_slave_t *, uint8_t *); void ec_master_output_stats(ec_master_t *); diff -r 05622513f627 -r 9dc190591c0f master/module.c --- a/master/module.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/module.c Wed Aug 02 12:25:25 2006 +0000 @@ -67,7 +67,7 @@ /*****************************************************************************/ static int ec_master_count = 1; /**< parameter value, number of masters */ -static int ec_eoe_devices = 0; /**< parameter value, number of EoE interf. */ +static int ec_eoeif_count = 0; /**< parameter value, number of EoE interf. */ static struct list_head ec_masters; /**< list of masters */ /*****************************************************************************/ @@ -75,14 +75,14 @@ /** \cond */ module_param(ec_master_count, int, S_IRUGO); -module_param(ec_eoe_devices, int, S_IRUGO); +module_param(ec_eoeif_count, int, S_IRUGO); MODULE_AUTHOR("Florian Pose "); MODULE_DESCRIPTION("EtherCAT master driver module"); MODULE_LICENSE("GPL"); MODULE_VERSION(COMPILE_INFO); MODULE_PARM_DESC(ec_master_count, "number of EtherCAT masters to initialize"); -MODULE_PARM_DESC(ec_eoe_devices, "number of EoE devices per master"); +MODULE_PARM_DESC(ec_eoeif_count, "number of EoE interfaces per master"); /** \endcond */ @@ -117,7 +117,7 @@ goto out_free; } - if (ec_master_init(master, i, ec_eoe_devices)) + if (ec_master_init(master, i, ec_eoeif_count)) goto out_free; if (kobject_add(&master->kobj)) { @@ -350,7 +350,7 @@ Starts the master associated with the device. This function has to be called by the network device driver to tell the master that the device is ready to send and receive data. The master - will enter the free-run mode then. + will enter the idle mode then. \ingroup DeviceInterface */ @@ -364,7 +364,7 @@ return -1; } - ec_master_freerun_start(master); + ec_master_idle_start(master); return 0; } @@ -382,7 +382,7 @@ ec_master_t *master; if (!(master = ec_find_master(master_index))) return; - ec_master_freerun_stop(master); + ec_master_idle_stop(master); if (ec_device_close(master->device)) EC_WARN("Failed to close device!\n"); @@ -424,7 +424,7 @@ goto out_release; } - ec_master_freerun_stop(master); + ec_master_idle_stop(master); ec_master_reset(master); master->mode = EC_MASTER_MODE_RUNNING; @@ -441,7 +441,7 @@ out_module_put: module_put(master->device->module); ec_master_reset(master); - ec_master_freerun_start(master); + ec_master_idle_start(master); out_release: master->reserved = 0; out_return: @@ -466,9 +466,7 @@ } ec_master_reset(master); - - master->mode = EC_MASTER_MODE_IDLE; - ec_master_freerun_start(master); + ec_master_idle_start(master); module_put(master->device->module); master->reserved = 0; diff -r 05622513f627 -r 9dc190591c0f master/slave.c --- a/master/slave.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/slave.c Wed Aug 02 12:25:25 2006 +0000 @@ -43,7 +43,7 @@ #include "globals.h" #include "slave.h" -#include "command.h" +#include "datagram.h" #include "master.h" /*****************************************************************************/ @@ -62,22 +62,22 @@ /** \cond */ EC_SYSFS_READ_ATTR(ring_position); -EC_SYSFS_READ_ATTR(coupler_address); -EC_SYSFS_READ_ATTR(vendor_name); -EC_SYSFS_READ_ATTR(product_name); -EC_SYSFS_READ_ATTR(product_desc); -EC_SYSFS_READ_ATTR(sii_name); -EC_SYSFS_READ_ATTR(type); +EC_SYSFS_READ_ATTR(advanced_position); +EC_SYSFS_READ_ATTR(vendor_name); // deprecated +EC_SYSFS_READ_ATTR(product_name); // deprecated +EC_SYSFS_READ_ATTR(product_desc); // deprecated +EC_SYSFS_READ_ATTR(name); +EC_SYSFS_READ_ATTR(type); // deprecated EC_SYSFS_READ_WRITE_ATTR(state); EC_SYSFS_READ_WRITE_ATTR(eeprom); static struct attribute *def_attrs[] = { &attr_ring_position, - &attr_coupler_address, + &attr_advanced_position, &attr_vendor_name, &attr_product_name, &attr_product_desc, - &attr_sii_name, + &attr_name, &attr_type, &attr_state, &attr_eeprom, @@ -155,7 +155,7 @@ slave->eeprom_name = NULL; slave->requested_state = EC_SLAVE_STATE_UNKNOWN; slave->current_state = EC_SLAVE_STATE_UNKNOWN; - slave->state_error = 0; + slave->error_flag = 0; slave->online = 1; slave->new_eeprom_data = NULL; slave->new_eeprom_size = 0; @@ -259,38 +259,40 @@ int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT slave */) { - ec_command_t *command; + ec_datagram_t *datagram; unsigned int i; uint16_t dl_status; - command = &slave->master->simple_command; + datagram = &slave->master->simple_datagram; // read base data - if (ec_command_nprd(command, slave->station_address, 0x0000, 6)) return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_nprd(datagram, slave->station_address, 0x0000, 6)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Reading base data from slave %i failed!\n", slave->ring_position); return -1; } - slave->base_type = EC_READ_U8 (command->data); - slave->base_revision = EC_READ_U8 (command->data + 1); - slave->base_build = EC_READ_U16(command->data + 2); - slave->base_fmmu_count = EC_READ_U8 (command->data + 4); - slave->base_sync_count = EC_READ_U8 (command->data + 5); + slave->base_type = EC_READ_U8 (datagram->data); + slave->base_revision = EC_READ_U8 (datagram->data + 1); + slave->base_build = EC_READ_U16(datagram->data + 2); + slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); + slave->base_sync_count = EC_READ_U8 (datagram->data + 5); if (slave->base_fmmu_count > EC_MAX_FMMUS) slave->base_fmmu_count = EC_MAX_FMMUS; // read data link status - if (ec_command_nprd(command, slave->station_address, 0x0110, 2)) return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Reading DL status from slave %i failed!\n", slave->ring_position); return -1; } - dl_status = EC_READ_U16(command->data); + dl_status = EC_READ_U16(datagram->data); for (i = 0; i < 4; i++) { slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0; slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0; @@ -342,17 +344,18 @@ /**< target memory */ ) { - ec_command_t *command; + ec_datagram_t *datagram; cycles_t start, end, timeout; - command = &slave->master->simple_command; + datagram = &slave->master->simple_datagram; // initiate read operation - if (ec_command_npwr(command, slave->station_address, 0x502, 6)) return -1; - EC_WRITE_U8 (command->data, 0x00); // read-only access - EC_WRITE_U8 (command->data + 1, 0x01); // request read operation - EC_WRITE_U32(command->data + 2, offset); - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_npwr(datagram, slave->station_address, 0x502, 6)) + return -1; + EC_WRITE_U8 (datagram->data, 0x00); // read-only access + EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation + EC_WRITE_U32(datagram->data + 2, offset); + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("SII-read failed on slave %i!\n", slave->ring_position); return -1; } @@ -364,9 +367,9 @@ { udelay(10); - if (ec_command_nprd(command, slave->station_address, 0x502, 10)) - return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_nprd(datagram, slave->station_address, 0x502, 10)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Getting SII-read status failed on slave %i!\n", slave->ring_position); return -1; @@ -375,8 +378,8 @@ end = get_cycles(); // check for "busy bit" - if (likely((EC_READ_U8(command->data + 1) & 0x81) == 0)) { - *target = EC_READ_U16(command->data + 6); + if (likely((EC_READ_U8(datagram->data + 1) & 0x81) == 0)) { + *target = EC_READ_U16(datagram->data + 6); return 0; } @@ -402,17 +405,18 @@ /**< target memory */ ) { - ec_command_t *command; + ec_datagram_t *datagram; cycles_t start, end, timeout; - command = &slave->master->simple_command; + datagram = &slave->master->simple_datagram; // initiate read operation - if (ec_command_npwr(command, slave->station_address, 0x502, 6)) return -1; - EC_WRITE_U8 (command->data, 0x00); // read-only access - EC_WRITE_U8 (command->data + 1, 0x01); // request read operation - EC_WRITE_U32(command->data + 2, offset); - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_npwr(datagram, slave->station_address, 0x502, 6)) + return -1; + EC_WRITE_U8 (datagram->data, 0x00); // read-only access + EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation + EC_WRITE_U32(datagram->data + 2, offset); + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("SII-read failed on slave %i!\n", slave->ring_position); return -1; } @@ -424,9 +428,9 @@ { udelay(10); - if (ec_command_nprd(command, slave->station_address, 0x502, 10)) - return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_nprd(datagram, slave->station_address, 0x502, 10)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Getting SII-read status failed on slave %i!\n", slave->ring_position); return -1; @@ -435,8 +439,8 @@ end = get_cycles(); // check "busy bit" - if (likely((EC_READ_U8(command->data + 1) & 0x81) == 0)) { - *target = EC_READ_U32(command->data + 6); + if (likely((EC_READ_U8(datagram->data + 1) & 0x81) == 0)) { + *target = EC_READ_U32(datagram->data + 6); return 0; } @@ -462,21 +466,22 @@ /**< new value */ ) { - ec_command_t *command; + ec_datagram_t *datagram; cycles_t start, end, timeout; - command = &slave->master->simple_command; + datagram = &slave->master->simple_datagram; EC_INFO("SII-write (slave %i, offset 0x%04X, value 0x%04X)\n", slave->ring_position, offset, value); // initiate write operation - if (ec_command_npwr(command, slave->station_address, 0x502, 8)) return -1; - EC_WRITE_U8 (command->data, 0x01); // enable write access - EC_WRITE_U8 (command->data + 1, 0x02); // request write operation - EC_WRITE_U32(command->data + 2, offset); - EC_WRITE_U16(command->data + 6, value); - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_npwr(datagram, slave->station_address, 0x502, 8)) + return -1; + EC_WRITE_U8 (datagram->data, 0x01); // enable write access + EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation + EC_WRITE_U32(datagram->data + 2, offset); + EC_WRITE_U16(datagram->data + 6, value); + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("SII-write failed on slave %i!\n", slave->ring_position); return -1; } @@ -488,9 +493,9 @@ { udelay(10); - if (ec_command_nprd(command, slave->station_address, 0x502, 2)) - return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_nprd(datagram, slave->station_address, 0x502, 2)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Getting SII-write status failed on slave %i!\n", slave->ring_position); return -1; @@ -499,8 +504,8 @@ end = get_cycles(); // check "busy bit" - if (likely((EC_READ_U8(command->data + 1) & 0x82) == 0)) { - if (EC_READ_U8(command->data + 1) & 0x40) { + if (likely((EC_READ_U8(datagram->data + 1) & 0x82) == 0)) { + if (EC_READ_U8(datagram->data + 1) & 0x40) { EC_ERR("SII-write failed!\n"); return -1; } @@ -534,7 +539,7 @@ word_offset = 0x0040; - if (!(cat_data = (uint8_t *) kmalloc(0x10000, GFP_KERNEL))) { + if (!(cat_data = (uint8_t *) kmalloc(0x10000, GFP_ATOMIC))) { EC_ERR("Failed to allocate 64k bytes for category data.\n"); return -1; } @@ -632,7 +637,7 @@ size = data[offset]; // allocate memory for string structure and data at a single blow if (!(string = (ec_eeprom_string_t *) - kmalloc(sizeof(ec_eeprom_string_t) + size + 1, GFP_KERNEL))) { + kmalloc(sizeof(ec_eeprom_string_t) + size + 1, GFP_ATOMIC))) { EC_ERR("Failed to allocate string memory.\n"); return -1; } @@ -696,7 +701,7 @@ for (i = 0; i < sync_count; i++, data += 8) { if (!(sync = (ec_eeprom_sync_t *) - kmalloc(sizeof(ec_eeprom_sync_t), GFP_KERNEL))) { + kmalloc(sizeof(ec_eeprom_sync_t), GFP_ATOMIC))) { EC_ERR("Failed to allocate Sync-Manager memory.\n"); return -1; } @@ -732,7 +737,7 @@ while (word_count >= 4) { if (!(pdo = (ec_eeprom_pdo_t *) - kmalloc(sizeof(ec_eeprom_pdo_t), GFP_KERNEL))) { + kmalloc(sizeof(ec_eeprom_pdo_t), GFP_ATOMIC))) { EC_ERR("Failed to allocate PDO memory.\n"); return -1; } @@ -753,7 +758,7 @@ for (i = 0; i < entry_count; i++) { if (!(entry = (ec_eeprom_pdo_entry_t *) - kmalloc(sizeof(ec_eeprom_pdo_entry_t), GFP_KERNEL))) { + kmalloc(sizeof(ec_eeprom_pdo_entry_t), GFP_ATOMIC))) { EC_ERR("Failed to allocate PDO entry memory.\n"); return -1; } @@ -803,7 +808,7 @@ list_for_each_entry(string, &slave->eeprom_strings, list) { if (--index) continue; - if (!(*ptr = (char *) kmalloc(string->size + 1, GFP_KERNEL))) { + if (!(*ptr = (char *) kmalloc(string->size + 1, GFP_ATOMIC))) { EC_ERR("Unable to allocate string memory.\n"); return -1; } @@ -815,7 +820,7 @@ err_string = "(string not found)"; - if (!(*ptr = (char *) kmalloc(strlen(err_string) + 1, GFP_KERNEL))) { + if (!(*ptr = (char *) kmalloc(strlen(err_string) + 1, GFP_ATOMIC))) { EC_ERR("Unable to allocate string memory.\n"); return -1; } @@ -834,29 +839,28 @@ uint8_t state /**< previous state */ ) { - ec_command_t *command; + ec_datagram_t *datagram; cycles_t start, end, timeout; - command = &slave->master->simple_command; - - if (ec_command_npwr(command, slave->station_address, 0x0120, 2)) return; - EC_WRITE_U16(command->data, state | EC_ACK); - if (unlikely(ec_master_simple_io(slave->master, command))) { + datagram = &slave->master->simple_datagram; + + if (ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2)) return; + EC_WRITE_U16(datagram->data, state | EC_ACK); + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_WARN("Acknowledge sending failed on slave %i!\n", slave->ring_position); return; } start = get_cycles(); - timeout = (cycles_t) 10 * cpu_khz; // 10ms - - while (1) - { + timeout = (cycles_t) 100 * cpu_khz; // 100ms + + while (1) { udelay(100); // wait a little bit... - if (ec_command_nprd(command, slave->station_address, 0x0130, 2)) + if (ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2)) return; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (unlikely(ec_master_simple_io(slave->master, datagram))) { slave->current_state = EC_SLAVE_STATE_UNKNOWN; EC_WARN("Acknowledge checking failed on slave %i!\n", slave->ring_position); @@ -865,14 +869,14 @@ end = get_cycles(); - if (likely(EC_READ_U8(command->data) == state)) { + if (EC_READ_U8(datagram->data) == state) { slave->current_state = state; EC_INFO("Acknowleged state 0x%02X on slave %i.\n", state, slave->ring_position); return; } - if (unlikely((end - start) >= timeout)) { + if (end - start >= timeout) { slave->current_state = EC_SLAVE_STATE_UNKNOWN; EC_WARN("Failed to acknowledge state 0x%02X on slave %i" " - Timeout!\n", state, slave->ring_position); @@ -891,20 +895,20 @@ void ec_slave_read_al_status_code(ec_slave_t *slave /**< EtherCAT slave */) { - ec_command_t *command; + ec_datagram_t *datagram; uint16_t code; const ec_code_msg_t *al_msg; - command = &slave->master->simple_command; - - if (ec_command_nprd(command, slave->station_address, 0x0134, 2)) return; - if (unlikely(ec_master_simple_io(slave->master, command))) { + datagram = &slave->master->simple_datagram; + + if (ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2)) return; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_WARN("Failed to read AL status code on slave %i!\n", slave->ring_position); return; } - if (!(code = EC_READ_U16(command->data))) return; + if (!(code = EC_READ_U16(datagram->data))) return; for (al_msg = al_status_messages; al_msg->code; al_msg++) { if (al_msg->code == code) { @@ -928,16 +932,17 @@ uint8_t state /**< new state */ ) { - ec_command_t *command; + ec_datagram_t *datagram; cycles_t start, end, timeout; - command = &slave->master->simple_command; + datagram = &slave->master->simple_datagram; slave->requested_state = state; - if (ec_command_npwr(command, slave->station_address, 0x0120, 2)) return -1; - EC_WRITE_U16(command->data, state); - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2)) + return -1; + EC_WRITE_U16(datagram->data, state); + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_ERR("Failed to set state 0x%02X on slave %i!\n", state, slave->ring_position); return -1; @@ -950,9 +955,9 @@ { udelay(100); // wait a little bit - if (ec_command_nprd(command, slave->station_address, 0x0130, 2)) - return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { slave->current_state = EC_SLAVE_STATE_UNKNOWN; EC_ERR("Failed to check state 0x%02X on slave %i!\n", state, slave->ring_position); @@ -961,18 +966,19 @@ end = get_cycles(); - if (unlikely(EC_READ_U8(command->data) & 0x10)) { // state change error + if (unlikely(EC_READ_U8(datagram->data) & 0x10)) { + // state change error EC_ERR("Failed to set state 0x%02X - Slave %i refused state change" " (code 0x%02X)!\n", state, slave->ring_position, - EC_READ_U8(command->data)); - slave->current_state = EC_READ_U8(command->data); + EC_READ_U8(datagram->data)); + slave->current_state = EC_READ_U8(datagram->data); state = slave->current_state & 0x0F; ec_slave_read_al_status_code(slave); ec_slave_state_ack(slave, state); return -1; } - if (likely(EC_READ_U8(command->data) == (state & 0x0F))) { + if (likely(EC_READ_U8(datagram->data) == (state & 0x0F))) { slave->current_state = state; return 0; // state change successful } @@ -1205,44 +1211,46 @@ int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT slave */) { - ec_command_t *command; - - command = &slave->master->simple_command; - - if (ec_command_nprd(command, slave->station_address, 0x0300, 4)) return -1; - if (unlikely(ec_master_simple_io(slave->master, command))) { + ec_datagram_t *datagram; + + datagram = &slave->master->simple_datagram; + + if (ec_datagram_nprd(datagram, slave->station_address, 0x0300, 4)) + return -1; + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_WARN("Reading CRC fault counters failed on slave %i!\n", slave->ring_position); return -1; } - if (!EC_READ_U32(command->data)) return 0; // no CRC faults - - if (EC_READ_U8(command->data)) + if (!EC_READ_U32(datagram->data)) return 0; // no CRC faults + + if (EC_READ_U8(datagram->data)) EC_WARN("%3i RX-error%s on slave %i, channel A.\n", - EC_READ_U8(command->data), - EC_READ_U8(command->data) == 1 ? "" : "s", + EC_READ_U8(datagram->data), + EC_READ_U8(datagram->data) == 1 ? "" : "s", slave->ring_position); - if (EC_READ_U8(command->data + 1)) + if (EC_READ_U8(datagram->data + 1)) EC_WARN("%3i invalid frame%s on slave %i, channel A.\n", - EC_READ_U8(command->data + 1), - EC_READ_U8(command->data + 1) == 1 ? "" : "s", + EC_READ_U8(datagram->data + 1), + EC_READ_U8(datagram->data + 1) == 1 ? "" : "s", slave->ring_position); - if (EC_READ_U8(command->data + 2)) + if (EC_READ_U8(datagram->data + 2)) EC_WARN("%3i RX-error%s on slave %i, channel B.\n", - EC_READ_U8(command->data + 2), - EC_READ_U8(command->data + 2) == 1 ? "" : "s", + EC_READ_U8(datagram->data + 2), + EC_READ_U8(datagram->data + 2) == 1 ? "" : "s", slave->ring_position); - if (EC_READ_U8(command->data + 3)) + if (EC_READ_U8(datagram->data + 3)) EC_WARN("%3i invalid frame%s on slave %i, channel B.\n", - EC_READ_U8(command->data + 3), - EC_READ_U8(command->data + 3) == 1 ? "" : "s", + EC_READ_U8(datagram->data + 3), + EC_READ_U8(datagram->data + 3) == 1 ? "" : "s", slave->ring_position); // reset CRC counters - if (ec_command_npwr(command, slave->station_address, 0x0300, 4)) return -1; - EC_WRITE_U32(command->data, 0x00000000); - if (unlikely(ec_master_simple_io(slave->master, command))) { + if (ec_datagram_npwr(datagram, slave->station_address, 0x0300, 4)) + return -1; + EC_WRITE_U32(datagram->data, 0x00000000); + if (unlikely(ec_master_simple_io(slave->master, datagram))) { EC_WARN("Resetting CRC fault counters failed on slave %i!\n", slave->ring_position); return -1; @@ -1273,8 +1281,8 @@ return -1; } - if (slave->master->mode != EC_MASTER_MODE_FREERUN) { - EC_ERR("Writing EEPROMs only allowed in freerun mode!\n"); + if (slave->master->mode != EC_MASTER_MODE_IDLE) { + EC_ERR("Writing EEPROMs only allowed in idle mode!\n"); return -1; } @@ -1345,7 +1353,7 @@ if (attr == &attr_ring_position) { return sprintf(buffer, "%i\n", slave->ring_position); } - else if (attr == &attr_coupler_address) { + else if (attr == &attr_advanced_position) { return sprintf(buffer, "%i:%i\n", slave->coupler_index, slave->coupler_subindex); } @@ -1361,7 +1369,7 @@ if (slave->type) return sprintf(buffer, "%s\n", slave->type->description); } - else if (attr == &attr_sii_name) { + else if (attr == &attr_name) { if (slave->eeprom_name) return sprintf(buffer, "%s\n", slave->eeprom_name); } @@ -1369,6 +1377,8 @@ if (slave->type) { if (slave->type->special == EC_TYPE_BUS_COUPLER) return sprintf(buffer, "coupler\n"); + else if (slave->type->special == EC_TYPE_INFRA) + return sprintf(buffer, "infrastructure\n"); else return sprintf(buffer, "normal\n"); } @@ -1420,28 +1430,23 @@ ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj); if (attr == &attr_state) { - if (!strcmp(buffer, "INIT\n")) { + if (!strcmp(buffer, "INIT\n")) slave->requested_state = EC_SLAVE_STATE_INIT; - slave->state_error = 0; - return size; - } - else if (!strcmp(buffer, "PREOP\n")) { + else if (!strcmp(buffer, "PREOP\n")) slave->requested_state = EC_SLAVE_STATE_PREOP; - slave->state_error = 0; - return size; - } - else if (!strcmp(buffer, "SAVEOP\n")) { + else if (!strcmp(buffer, "SAVEOP\n")) slave->requested_state = EC_SLAVE_STATE_SAVEOP; - slave->state_error = 0; - return size; - } - else if (!strcmp(buffer, "OP\n")) { + else if (!strcmp(buffer, "OP\n")) slave->requested_state = EC_SLAVE_STATE_OP; - slave->state_error = 0; - return size; - } - - EC_ERR("Failed to set slave state!\n"); + else { + EC_ERR("Invalid slave state \"%s\"!\n", buffer); + return -EINVAL; + } + + EC_INFO("Accepted new state %s for slave %i.\n", + buffer, slave->ring_position); + slave->error_flag = 0; + return size; } else if (attr == &attr_eeprom) { if (!ec_slave_write_eeprom(slave, buffer, size)) @@ -1488,6 +1493,40 @@ return size; } +/*****************************************************************************/ + +/** + Calculates the size of a sync manager by evaluating PDO sizes. + \return sync manager size +*/ + +uint16_t ec_slave_calc_eeprom_sync_size(const ec_slave_t *slave, + /**< EtherCAT slave */ + const ec_eeprom_sync_t *sync + /**< sync manager */ + ) +{ + ec_eeprom_pdo_t *pdo; + ec_eeprom_pdo_entry_t *pdo_entry; + unsigned int bit_size; + + if (sync->length) return sync->length; + + bit_size = 0; + list_for_each_entry(pdo, &slave->eeprom_pdos, list) { + if (pdo->sync_manager != sync->index) continue; + + list_for_each_entry(pdo_entry, &pdo->entries, list) { + bit_size += pdo_entry->bit_length; + } + } + + if (bit_size % 8) // round up to full bytes + return bit_size / 8 + 1; + else + return bit_size / 8; +} + /****************************************************************************** * Realtime interface *****************************************************************************/ diff -r 05622513f627 -r 9dc190591c0f master/slave.h --- a/master/slave.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/slave.h Wed Aug 02 12:25:25 2006 +0000 @@ -45,7 +45,7 @@ #include #include "globals.h" -#include "command.h" +#include "datagram.h" #include "types.h" /*****************************************************************************/ @@ -292,7 +292,7 @@ ec_slave_state_t requested_state; /**< requested slave state */ ec_slave_state_t current_state; /**< current slave state */ - unsigned int state_error; /**< a state error has happened */ + unsigned int error_flag; /**< stop processing after an error */ unsigned int online; /**< non-zero, if the slave responds. */ struct list_head varsize_fields; /**< size information for variable-sized @@ -326,6 +326,9 @@ // misc. size_t ec_slave_calc_sync_size(const ec_slave_t *, const ec_sync_t *); +uint16_t ec_slave_calc_eeprom_sync_size(const ec_slave_t *, + const ec_eeprom_sync_t *); + void ec_slave_print(const ec_slave_t *, unsigned int); int ec_slave_check_crc(ec_slave_t *); diff -r 05622513f627 -r 9dc190591c0f master/types.c --- a/master/types.c Mon Jun 26 16:07:07 2006 +0000 +++ b/master/types.c Wed Aug 02 12:25:25 2006 +0000 @@ -61,7 +61,7 @@ /*****************************************************************************/ const ec_slave_type_t Beckhoff_EK1110 = { - "Beckhoff", "EK1110", "Extension terminal", EC_TYPE_NORMAL, + "Beckhoff", "EK1110", "Extension terminal", EC_TYPE_INFRA, {NULL} // no sync managers }; diff -r 05622513f627 -r 9dc190591c0f master/types.h --- a/master/types.h Mon Jun 26 16:07:07 2006 +0000 +++ b/master/types.h Wed Aug 02 12:25:25 2006 +0000 @@ -60,6 +60,7 @@ { EC_TYPE_NORMAL, /**< no special slave */ EC_TYPE_BUS_COUPLER, /**< slave is a bus coupler */ + EC_TYPE_INFRA, /**< infrastructure slaves, that contain no process data */ EC_TYPE_EOE /**< slave is an EoE switch */ } ec_special_type_t; diff -r 05622513f627 -r 9dc190591c0f script/ec_list.pl --- a/script/ec_list.pl Mon Jun 26 16:07:07 2006 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,192 +0,0 @@ -#!/usr/bin/perl - -#------------------------------------------------------------------------------ -# -# e c _ l i s t . p l -# -# Userspace tool for listing EtherCAT slaves. -# -# $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. -# -#------------------------------------------------------------------------------ - -use strict; -use Getopt::Std; - -my $master_index; -my $master_dir; -my $show_sii_naming; - -#------------------------------------------------------------------------------ - -&get_options; -&query_master; -exit 0; - -#------------------------------------------------------------------------------ - -sub query_master -{ - $master_dir = "/sys/ethercat" . $master_index; - &query_slaves; -} - -#------------------------------------------------------------------------------ - -sub query_slaves -{ - my $dirhandle; - my $slave_dir; - my $entry; - my $slave_index; - my $file_name; - my $vendor_name; - my @slaves; - my $slave; - my $abs; - - unless (opendir $dirhandle, $master_dir) { - print "Failed to open directory \"$master_dir\".\n"; - exit 1; - } - - while ($entry = readdir $dirhandle) { - next unless $entry =~ /^slave(\d+)$/; - $slave_dir = $master_dir . "/" . $entry; - - $slave = {}; - $slave->{'ring_position'} = - &read_integer("$slave_dir/ring_position"); - $slave->{'coupler_address'} = - &read_string("$slave_dir/coupler_address"); - unless ($show_sii_naming) { - $slave->{'vendor_name'} = - &read_string("$slave_dir/vendor_name"); - $slave->{'product_name'} = - &read_string("$slave_dir/product_name"); - $slave->{'product_desc'} = - &read_string("$slave_dir/product_desc"); - } - else { - $slave->{'sii_name'} = - &read_string("$slave_dir/sii_name"); - } - $slave->{'type'} = - &read_string("$slave_dir/type"); - - push @slaves, $slave; - } - closedir $dirhandle; - - @slaves = sort { $a->{'ring_position'} <=> $b->{'ring_position'} } @slaves; - - print "EtherCAT bus listing for master $master_index:\n"; - for $slave (@slaves) { - if ($slave->{'type'} eq "coupler") { - print "--------------------------------------------------------\n"; - } - - $abs = sprintf "%i", $slave->{'ring_position'}; - printf(" %3s %8s ", $abs, $slave->{'coupler_address'}); - unless ($show_sii_naming) { - printf("%-12s %-10s %s\n", $slave->{'vendor_name'}, - $slave->{'product_name'}, $slave->{'product_desc'}); - } - else { - printf("%s\n", $slave->{'sii_name'}); - } - } -} - -#------------------------------------------------------------------------------ - -sub read_string -{ - (my $file_name) = @_; - my $data; - - $data = `cat $file_name 2>/dev/null`; - if ($?) { - print "ERROR: Unable to read string $file_name!\n"; - exit 1; - } - - chomp $data; - return $data; -} - -#------------------------------------------------------------------------------ - -sub read_integer -{ - (my $file_name) = @_; - - if (`cat $file_name 2>/dev/null` !~ /^(\d+)$/) { - print "ERROR: Unable to read integer $file_name!\n"; - exit 1; - } - - return int $1; -} - -#------------------------------------------------------------------------------ - -sub get_options -{ - my %opt; - my $optret; - - $optret = getopts "m:sh", \%opt; - - &print_usage if defined $opt{'h'} or $#ARGV > -1 or !$optret; - - if (defined $opt{'m'}) { - $master_index = $opt{'m'}; - } - else { - $master_index = 0; - } - - $show_sii_naming = defined $opt{'s'}; -} - -#------------------------------------------------------------------------------ - -sub print_usage -{ - print "Usage: ec_list [OPTIONS]\n"; - print " -m Query master .\n"; - print " -s Show SII naming instead of"; - print " vendor/product/description.\n"; - print " -h Show this help.\n"; - exit 0; -} - -#------------------------------------------------------------------------------ diff -r 05622513f627 -r 9dc190591c0f script/ethercat.sh --- a/script/ethercat.sh Mon Jun 26 16:07:07 2006 +0000 +++ b/script/ethercat.sh Wed Aug 02 12:25:25 2006 +0000 @@ -59,6 +59,80 @@ #------------------------------------------------------------------------------ +# +# Function for setting up the EoE bridge +# +build_eoe_bridge() { + if [ -z "$EOE_BRIDGE" ]; then return; fi + + EOEIF=`/sbin/ifconfig -a | grep -o -E "^eoe[0-9]+ "` + + # add bridge, if it does not already exist + if ! /sbin/brctl show | grep -E -q "^$EOE_BRIDGE"; then + if ! /sbin/brctl addbr $EOE_BRIDGE; then + /bin/false + rc_status -v + rc_exit + fi + fi + + # check if specified interfaces are bridged + for interf in $EOEIF $EOE_EXTRA_INTERFACES; do + # interface is already part of the bridge + if /sbin/brctl show $EOE_BRIDGE | grep -E -q $interf + then continue + fi + # clear IP address and open interface + if ! /sbin/ifconfig $interf 0.0.0.0 up; then + /bin/false + rc_status -v + rc_exit + fi + # add interface to the bridge + if ! /sbin/brctl addif $EOE_BRIDGE $interf; then + /bin/false + rc_status -v + rc_exit + fi + done + + # configure IP on bridge + if [ -n "$EOE_IP_ADDRESS" -a -n "$EOE_IP_NETMASK" ]; then + if ! /sbin/ifconfig $EOE_BRIDGE $EOE_IP_ADDRESS \ + netmask $EOE_IP_NETMASK; then + /bin/false + rc_status -v + rc_exit + fi + fi + + # open bridge + if ! /sbin/ifconfig $EOE_BRIDGE up; then + /bin/false + rc_status -v + rc_exit + fi + + # install new default gateway + if [ -n "$EOE_GATEWAY" ]; then + while /sbin/route -n | grep -E -q "^0.0.0.0"; do + if ! /sbin/route del default; then + echo "Failed to remove route!" 1>&2 + /bin/false + rc_status -v + rc_exit + fi + done + if ! /sbin/route add default gw $EOE_GATEWAY; then + /bin/false + rc_status -v + rc_exit + fi + fi +} + +#------------------------------------------------------------------------------ + . /etc/rc.status rc_reset @@ -73,10 +147,15 @@ rc_exit fi - if [ -z "$EOE_DEVICES" ]; then - EOE_DEVICES=0 - fi - + if [ -z "$EOE_INTERFACES" ]; then + if [ -n "$EOE_DEVICES"]; then # support legacy sysconfig files + EOE_INTERFACES=$EOE_DEVICES + else + EOE_INTERFACES=0 + fi + fi + + # unload conflicting modules at first for mod in 8139too 8139cp; do if lsmod | grep "^$mod " > /dev/null; then if ! rmmod $mod; then @@ -87,78 +166,22 @@ fi done - if ! modprobe ec_master ec_eoe_devices=$EOE_DEVICES; then + # load master module + if ! modprobe ec_master ec_eoeif_count=$EOE_INTERFACES; then /bin/false rc_status -v rc_exit fi + # load device module if ! modprobe ec_8139too ec_device_index=$DEVICE_INDEX; then /bin/false rc_status -v rc_exit fi - # Build EoE bridge - if [ -n "$EOE_BRIDGE" ]; then - - EOE_INTERFACES=`/sbin/ifconfig -a | grep -o -E "^eoe[0-9]+ "` - - # add bridge, if it does not already exist - if ! /sbin/brctl show | grep -E -q "^$EOE_BRIDGE"; then - if ! /sbin/brctl addbr $EOE_BRIDGE; then - /bin/false - rc_status -v - rc_exit - fi - fi - - # free all interfaces of their addresses and add them to the bridge - for interface in $EOE_INTERFACES $EOE_EXTRA_INTERFACES; do - if ! /sbin/ifconfig $interface 0.0.0.0 up; then - /bin/false - rc_status -v - rc_exit - fi - if ! /sbin/brctl show | grep -E -q "^$EOE_BRIDGE.*$interface"; then - if ! /sbin/brctl addif $EOE_BRIDGE $interface; then - /bin/false - rc_status -v - rc_exit - fi - fi - done - if [ -n "$EOE_IP_ADDRESS" -a -n "$EOE_IP_NETMASK" ]; then - if ! /sbin/ifconfig $EOE_BRIDGE $EOE_IP_ADDRESS \ - netmask $EOE_IP_NETMASK; then - /bin/false - rc_status -v - rc_exit - fi - fi - if ! /sbin/ifconfig $EOE_BRIDGE up; then - /bin/false - rc_status -v - rc_exit - fi - - # install new default gateway - if [ -n "$EOE_GATEWAY" ]; then - while /sbin/route -n | grep -E -q "^0.0.0.0"; do - if ! /sbin/route del default; then - echo "Failed to remove default route!" 1>&2 - /bin/false - rc_status -v - rc_exit - fi - done - if ! /sbin/route add default gw $EOE_GATEWAY; then - /bin/false - rc_status -v - rc_exit - fi - fi - fi + # build EoE bridge + build_eoe_bridge rc_status -v ;; @@ -166,6 +189,7 @@ stop) echo -n "Shutting down EtherCAT master " + # unload modules for mod in ec_8139too ec_master; do if lsmod | grep "^$mod " > /dev/null; then if ! rmmod $mod; then @@ -176,6 +200,9 @@ fi; done + sleep 1 + + # reload previous modules if ! modprobe 8139too; then echo "Warning: Failed to restore 8139too module." fi @@ -184,7 +211,10 @@ ;; restart) - $0 stop + $0 stop || exit 1 + + sleep 1 + $0 start rc_status @@ -197,10 +227,24 @@ master_running=$? lsmod | grep "^ec_8139too " > /dev/null device_running=$? + + # master module and device module loaded? test $master_running -eq 0 -a $device_running -eq 0 rc_status -v ;; + + bridge) + echo -n "Building EoE bridge " + + build_eoe_bridge + + rc_status -v + ;; + + *) + echo "USAGE: $0 {start|stop|restart|status|bridge}" + ;; esac rc_exit diff -r 05622513f627 -r 9dc190591c0f script/install.sh --- a/script/install.sh Mon Jun 26 16:07:07 2006 +0000 +++ b/script/install.sh Wed Aug 02 12:25:25 2006 +0000 @@ -88,8 +88,10 @@ # Install tools echo " Installing tools" -cp script/ec_list.pl /usr/local/bin/ec_list || exit 1 +cp script/lsec.pl /usr/local/bin/lsec || exit 1 chmod +x /usr/local/bin/ec_list || exit 1 +rm -f /usr/local/bin/ec_list || exit 1 +ln -s /usr/local/bin/lsec /usr/local/bin/ec_list || exit 1 #------------------------------------------------------------------------------ diff -r 05622513f627 -r 9dc190591c0f script/lsec.pl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/script/lsec.pl Wed Aug 02 12:25:25 2006 +0000 @@ -0,0 +1,192 @@ +#!/usr/bin/perl + +#------------------------------------------------------------------------------ +# +# l s e c - List EtherCAT +# +# Userspace tool for listing EtherCAT slaves. +# +# $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. +# +#------------------------------------------------------------------------------ + +use strict; +use Getopt::Std; + +my $master_index; +my $master_dir; +my $show_sii_naming; + +#------------------------------------------------------------------------------ + +&get_options; +&query_master; +exit 0; + +#------------------------------------------------------------------------------ + +sub query_master +{ + $master_dir = "/sys/ethercat" . $master_index; + &query_slaves; +} + +#------------------------------------------------------------------------------ + +sub query_slaves +{ + my $dirhandle; + my $slave_dir; + my $entry; + my $slave_index; + my $file_name; + my $vendor_name; + my @slaves; + my $slave; + my $abs; + + unless (opendir $dirhandle, $master_dir) { + print "Failed to open directory \"$master_dir\".\n"; + exit 1; + } + + while ($entry = readdir $dirhandle) { + next unless $entry =~ /^slave(\d+)$/; + $slave_dir = $master_dir . "/" . $entry; + + $slave = {}; + $slave->{'ring_position'} = + &read_integer("$slave_dir/ring_position"); + $slave->{'advanced_position'} = + &read_string("$slave_dir/advanced_position"); + unless ($show_sii_naming) { + $slave->{'vendor_name'} = + &read_string("$slave_dir/vendor_name"); + $slave->{'product_name'} = + &read_string("$slave_dir/product_name"); + $slave->{'product_desc'} = + &read_string("$slave_dir/product_desc"); + } + else { + $slave->{'name'} = + &read_string("$slave_dir/name"); + } + $slave->{'type'} = + &read_string("$slave_dir/type"); + + push @slaves, $slave; + } + closedir $dirhandle; + + @slaves = sort { $a->{'ring_position'} <=> $b->{'ring_position'} } @slaves; + + print "EtherCAT bus listing for master $master_index:\n"; + for $slave (@slaves) { + if ($slave->{'type'} eq "coupler") { + print "--------------------------------------------------------\n"; + } + + $abs = sprintf "%i", $slave->{'ring_position'}; + printf(" %3s %8s ", $abs, $slave->{'advanced_position'}); + unless ($show_sii_naming) { + printf("%-12s %-10s %s\n", $slave->{'vendor_name'}, + $slave->{'product_name'}, $slave->{'product_desc'}); + } + else { + printf("%s\n", $slave->{'name'}); + } + } +} + +#------------------------------------------------------------------------------ + +sub read_string +{ + (my $file_name) = @_; + my $data; + + $data = `cat $file_name 2>/dev/null`; + if ($?) { + print "ERROR: Unable to read string $file_name!\n"; + exit 1; + } + + chomp $data; + return $data; +} + +#------------------------------------------------------------------------------ + +sub read_integer +{ + (my $file_name) = @_; + + if (`cat $file_name 2>/dev/null` !~ /^(\d+)$/) { + print "ERROR: Unable to read integer $file_name!\n"; + exit 1; + } + + return int $1; +} + +#------------------------------------------------------------------------------ + +sub get_options +{ + my %opt; + my $optret; + + $optret = getopts "m:sh", \%opt; + + &print_usage if defined $opt{'h'} or $#ARGV > -1 or !$optret; + + if (defined $opt{'m'}) { + $master_index = $opt{'m'}; + } + else { + $master_index = 0; + } + + $show_sii_naming = defined $opt{'s'}; +} + +#------------------------------------------------------------------------------ + +sub print_usage +{ + print "Usage: $0 [OPTIONS]\n"; + print " -m Query master .\n"; + print " -s Show EEPROM name instead of"; + print " vendor/product/description.\n"; + print " -h Show this help.\n"; + exit 0; +} + +#------------------------------------------------------------------------------ diff -r 05622513f627 -r 9dc190591c0f script/sysconfig --- a/script/sysconfig Mon Jun 26 16:07:07 2006 +0000 +++ b/script/sysconfig Wed Aug 02 12:25:25 2006 +0000 @@ -13,10 +13,10 @@ #DEVICE_INDEX=99 # -# Number of Ethernet-over-EtherCAT devices every master shall create +# Number of Ethernet-over-EtherCAT interfaces every master shall create # on startup. Default is 0. # -#EOE_DEVICES=0 +#EOE_INTERFACES=0 # # Bridge all EoE interfaces after master startup