# HG changeset patch # User Florian Pose # Date 1147780626 0 # Node ID b4960499098fb90c28d636be356536c0abbe23b8 # Parent 7b3126cf6dabf75536152987887974c0a2c3777f Added finite state machine (FSM) processing. diff -r 7b3126cf6dab -r b4960499098f master/Makefile --- a/master/Makefile Mon May 15 12:57:24 2006 +0000 +++ b/master/Makefile Tue May 16 11:57:06 2006 +0000 @@ -33,7 +33,7 @@ obj-m := ec_master.o ec_master-objs := module.o master.o device.o slave.o command.o types.o \ - domain.o mailbox.o canopen.o ethernet.o debug.o + domain.o mailbox.o canopen.o ethernet.o debug.o fsm.o REV := $(shell svnversion $(src) 2>/dev/null) diff -r 7b3126cf6dab -r b4960499098f master/command.h --- a/master/command.h Mon May 15 12:57:24 2006 +0000 +++ b/master/command.h Tue May 16 11:57:06 2006 +0000 @@ -115,6 +115,7 @@ 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); diff -r 7b3126cf6dab -r b4960499098f master/fsm.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm.c Tue May 16 11:57:06 2006 +0000 @@ -0,0 +1,787 @@ +/****************************************************************************** + * + * $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; version 2 of the License. + * + * 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 + * + *****************************************************************************/ + +/** + \file + EtherCAT finite state machines. +*/ + +/*****************************************************************************/ + +#include "globals.h" +#include "fsm.h" +#include "master.h" + +/*****************************************************************************/ + +#define EC_CAT_MEM 0x100 + +/*****************************************************************************/ + +void ec_fsm_master_start(ec_fsm_t *); +void ec_fsm_master_wait(ec_fsm_t *); +void ec_fsm_master_slave(ec_fsm_t *); +void ec_fsm_master_calc(ec_fsm_t *); +void ec_fsm_master_finished(ec_fsm_t *); + +void ec_fsm_slave_start(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_prepare_sii(ec_fsm_t *); +void ec_fsm_slave_read_sii(ec_fsm_t *); +void ec_fsm_slave_categories(ec_fsm_t *); +void ec_fsm_slave_category_header(ec_fsm_t *); +void ec_fsm_slave_category_data(ec_fsm_t *); +void ec_fsm_slave_finished(ec_fsm_t *); + +void ec_fsm_sii_start_reading(ec_fsm_t *); +void ec_fsm_sii_check(ec_fsm_t *); +void ec_fsm_sii_fetch(ec_fsm_t *); +void ec_fsm_sii_finished(ec_fsm_t *); +void ec_fsm_sii_error(ec_fsm_t *); + +/*****************************************************************************/ + +int ec_fsm_init(ec_fsm_t *fsm, ec_master_t *master) +{ + fsm->master = master; + fsm->master_state = ec_fsm_master_start; + fsm->master_slaves_responding = 0; + fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN; + fsm->slave_cat_data = NULL; + + ec_command_init(&fsm->command); + if (ec_command_prealloc(&fsm->command, EC_MAX_DATA_SIZE)) { + EC_ERR("FSM failed to allocate FSM command.\n"); + return -1; + } + + return 0; +} + +/*****************************************************************************/ + +void ec_fsm_clear(ec_fsm_t *fsm) +{ + if (fsm->slave_cat_data) kfree(fsm->slave_cat_data); + ec_command_clear(&fsm->command); +} + +/*****************************************************************************/ + +void ec_fsm_reset(ec_fsm_t *fsm) +{ + fsm->master_state = ec_fsm_master_start; + fsm->master_slaves_responding = 0; + fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN; + + if (fsm->slave_cat_data) { + kfree(fsm->slave_cat_data); + fsm->slave_cat_data = NULL; + } +} + +/*****************************************************************************/ + +void ec_fsm_execute(ec_fsm_t *fsm) +{ + fsm->master_state(fsm); +} + +/*****************************************************************************/ + +int ec_fsm_idle(const ec_fsm_t *fsm) +{ + return (fsm->master_state == ec_fsm_master_start || + fsm->master_state == ec_fsm_master_wait || + fsm->master_state == ec_fsm_master_finished); +} + +/****************************************************************************** + * master state machine + *****************************************************************************/ + +/** + State: Start. + Starts with getting slave count and slave states. +*/ + +void ec_fsm_master_start(ec_fsm_t *fsm) +{ + ec_command_brd(&fsm->command, 0x0130, 2); + ec_master_queue_command(fsm->master, &fsm->command); + + fsm->master_state = ec_fsm_master_wait; +} + +/*****************************************************************************/ + +void ec_fsm_master_wait(ec_fsm_t *fsm) +{ + ec_command_t *command = &fsm->command; + unsigned int first, topology_change, i; + ec_slave_t *slave; + + if (command->state != EC_CMD_RECEIVED) { + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + if (command->working_counter == fsm->master_slaves_responding && + command->data[0] == fsm->master_slave_states) { + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + topology_change = command->working_counter != + fsm->master_slaves_responding; + + fsm->master_slaves_responding = command->working_counter; + fsm->master_slave_states = command->data[0]; + + EC_INFO("FSM: %i slave%s responding (", fsm->master_slaves_responding, + fsm->master_slaves_responding == 1 ? "" : "s"); + + first = 1; + if (fsm->master_slave_states & EC_SLAVE_STATE_INIT) { + printk("INIT"); + first = 0; + } + if (fsm->master_slave_states & EC_SLAVE_STATE_PREOP) { + if (!first) printk(", "); + printk("PREOP"); + first = 0; + } + if (fsm->master_slave_states & EC_SLAVE_STATE_SAVEOP) { + if (!first) printk(", "); + printk("SAVEOP"); + first = 0; + } + if (fsm->master_slave_states & EC_SLAVE_STATE_OP) { + if (!first) printk(", "); + printk("OP"); + } + printk(")\n"); + + if (!topology_change || fsm->master->mode == EC_MASTER_MODE_RUNNING) { + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + // topology change! + ec_master_clear_slaves(fsm->master); + + if (!fsm->master_slaves_responding) { + // no slaves present -> finish state machine. + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately + return; + } + + // init slaves + for (i = 0; i < fsm->master_slaves_responding; i++) { + if (!(slave = + (ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_ATOMIC))) { + EC_ERR("FSM failed to allocate slave %i!\n", i); + fsm->master_state = ec_fsm_master_finished; + return; + } + + if (ec_slave_init(slave, fsm->master, i, i + 1)) { + fsm->master_state = ec_fsm_master_finished; + return; + } + + if (kobject_add(&slave->kobj)) { + EC_ERR("FSM failed to add kobject.\n"); + kobject_put(&slave->kobj); // free + fsm->master_state = ec_fsm_master_finished; + return; + } + + list_add_tail(&slave->list, &fsm->master->slaves); + } + + // begin scanning of slaves + fsm->slave = list_entry(fsm->master->slaves.next, + ec_slave_t, list); + fsm->slave_state = ec_fsm_slave_start; + + fsm->master_state = ec_fsm_master_slave; + fsm->master_state(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + State: Get Slave. + Executes the sub-statemachine of a slave. +*/ + +void ec_fsm_master_slave(ec_fsm_t *fsm) +{ + ec_master_t *master = fsm->master; + + fsm->slave_state(fsm); // execute slave state machine + + if (fsm->slave_state != ec_fsm_slave_finished) return; + + // have all slaves been fetched? + if (fsm->slave->list.next == &master->slaves) { + fsm->master_state = ec_fsm_master_calc; + fsm->master_state(fsm); // execute immediately + return; + } + + // process next slave + fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); + fsm->slave_state = ec_fsm_slave_start; + fsm->slave_state(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + Free-Run state: Calc. +*/ + +void ec_fsm_master_calc(ec_fsm_t *fsm) +{ + uint16_t coupler_index, coupler_subindex; + uint16_t reverse_coupler_index, current_coupler_index; + ec_slave_t *slave; + ec_slave_ident_t *ident; + ec_master_t *master = fsm->master; + + coupler_index = 0; + reverse_coupler_index = 0xFFFF; + current_coupler_index = 0x3FFF; + coupler_subindex = 0; + + // for every slave on the bus + list_for_each_entry(slave, &master->slaves, list) + { + // search for identification in "database" + ident = slave_idents; + while (ident->type) { + if (unlikely(ident->vendor_id == slave->sii_vendor_id + && ident->product_code == slave->sii_product_code)) { + slave->type = ident->type; + break; + } + ident++; + } + + if (!slave->type) { + EC_WARN("FSM: Unknown slave device (vendor 0x%08X, code 0x%08X) at" + " position %i.\n", slave->sii_vendor_id, + slave->sii_product_code, slave->ring_position); + } + else if (slave->type->special == EC_TYPE_BUS_COUPLER) { + if (slave->sii_alias) + current_coupler_index = reverse_coupler_index--; + else + current_coupler_index = coupler_index++; + coupler_subindex = 0; + } + + slave->coupler_index = current_coupler_index; + slave->coupler_subindex = coupler_subindex; + coupler_subindex++; + } + + fsm->master_state = ec_fsm_master_start; + fsm->master_state(fsm); // execute immediately +} + +/*****************************************************************************/ + +/** + Free-Run state: Finished. + End state of the state machine. Does nothing. +*/ + +void ec_fsm_master_finished(ec_fsm_t *fsm) +{ +} + +/****************************************************************************** + * slave state machine + *****************************************************************************/ + +/** + Slave state: Start. + First state of the slave state machine. Writes the station address to the + slave, according to its ring position. +*/ + +void ec_fsm_slave_start(ec_fsm_t *fsm) +{ + ec_command_t *command = &fsm->command; + + // 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_base; +} + +/*****************************************************************************/ + +/** + Slave state: Read base. +*/ + +void ec_fsm_slave_read_base(ec_fsm_t *fsm) +{ + 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", + fsm->slave->ring_position); + fsm->slave_state = ec_fsm_slave_finished; + return; + } + + // 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) +{ + 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", + slave->ring_position); + fsm->slave_state = ec_fsm_slave_finished; + 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); + + 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_prepare_sii; +} + +/*****************************************************************************/ + +/** + Slave state: Prepare SII. +*/ + +void ec_fsm_slave_prepare_sii(ec_fsm_t *fsm) +{ + ec_command_t *command = &fsm->command; + 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", + slave->ring_position); + fsm->slave_state = ec_fsm_slave_finished; + return; + } + + dl_status = EC_READ_U16(command->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; + slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0; + } + + fsm->sii_offset = 0x0004; + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->slave_sii_num = 0; + fsm->slave_state = ec_fsm_slave_read_sii; + fsm->slave_state(fsm); // execute state immediately +} + +/*****************************************************************************/ + +/** + Slave state: Read SII. +*/ + +void ec_fsm_slave_read_sii(ec_fsm_t *fsm) +{ + ec_slave_t *slave = fsm->slave; + + // execute SII state machine + fsm->sii_state(fsm); + + if (fsm->sii_state == ec_fsm_sii_error) { + fsm->slave_state = ec_fsm_slave_finished; + EC_ERR("FSM failed to read SII data at 0x%04X on slave %i.\n", + fsm->sii_offset, slave->ring_position); + return; + } + + if (fsm->sii_state != ec_fsm_sii_finished) return; + + switch (fsm->slave_sii_num) { + case 0: + slave->sii_alias = fsm->sii_result & 0xFFFF; + fsm->sii_offset = 0x0008; + break; + case 1: + slave->sii_vendor_id = fsm->sii_result; + fsm->sii_offset = 0x000A; + break; + case 2: + slave->sii_product_code = fsm->sii_result; + fsm->sii_offset = 0x000C; + break; + case 3: + slave->sii_revision_number = fsm->sii_result; + fsm->sii_offset = 0x000E; + break; + case 4: + slave->sii_serial_number = fsm->sii_result; + fsm->sii_offset = 0x0018; + break; + case 5: + slave->sii_rx_mailbox_offset = fsm->sii_result & 0xFFFF; + slave->sii_rx_mailbox_size = fsm->sii_result >> 16; + fsm->sii_offset = 0x001A; + break; + case 6: + slave->sii_tx_mailbox_offset = fsm->sii_result & 0xFFFF; + slave->sii_tx_mailbox_size = fsm->sii_result >> 16; + fsm->sii_offset = 0x001C; + break; + case 7: + slave->sii_mailbox_protocols = fsm->sii_result & 0xFFFF; + fsm->slave_state = ec_fsm_slave_categories; + fsm->slave_state(fsm); // execute state immediately + return; + } + + fsm->slave_sii_num++; + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->slave_state(fsm); // execute state immediately +} + +/*****************************************************************************/ + +/** + Slave state: Categories. +*/ + +void ec_fsm_slave_categories(ec_fsm_t *fsm) +{ + fsm->slave_cat_offset = 0x0040; + + if (fsm->slave_cat_data) { + EC_INFO("FSM freeing old category data on slave %i...\n", + fsm->slave->ring_position); + kfree(fsm->slave_cat_data); + } + + if (!(fsm->slave_cat_data = (uint8_t *) kmalloc(EC_CAT_MEM, GFP_ATOMIC))) { + EC_ERR("FSM Failed to allocate category data.\n"); + fsm->slave_state = ec_fsm_slave_finished; + return; + } + + // start reading first category header + fsm->sii_offset = fsm->slave_cat_offset; + fsm->sii_state = ec_fsm_sii_start_reading; + + fsm->slave_state = ec_fsm_slave_category_header; + fsm->slave_state(fsm); // execute state immediately +} + +/*****************************************************************************/ + +/** + Slave state: Read categories. + Start reading categories. +*/ + +void ec_fsm_slave_category_header(ec_fsm_t *fsm) +{ + // execute SII state machine + fsm->sii_state(fsm); + + if (fsm->sii_state == ec_fsm_sii_error) { + kfree(fsm->slave_cat_data); + fsm->slave_cat_data = NULL; + fsm->slave_state = ec_fsm_slave_finished; + EC_ERR("FSM failed to read category header at 0x%04X on slave %i.\n", + fsm->slave_cat_offset, fsm->slave->ring_position); + return; + } + + if (fsm->sii_state != ec_fsm_sii_finished) return; + + // last category? + if ((fsm->sii_result & 0xFFFF) == 0xFFFF) { + kfree(fsm->slave_cat_data); + fsm->slave_cat_data = NULL; + fsm->slave_state = ec_fsm_slave_finished; + return; + } + + fsm->slave_cat_type = fsm->sii_result & 0x7FFF; + fsm->slave_cat_words = (fsm->sii_result >> 16) & 0xFFFF; + + if (fsm->slave_cat_words > EC_CAT_MEM * 2) { + EC_ERR("FSM category memory too small! %i words needed.\n", + fsm->slave_cat_words); + fsm->slave_state = ec_fsm_slave_finished; + return; + } + + // start reading category data + fsm->slave_cat_data_offset = 0; + fsm->sii_offset = (fsm->slave_cat_offset + 2 + + fsm->slave_cat_data_offset); + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->slave_state = ec_fsm_slave_category_data; + fsm->slave_state(fsm); // execute state immediately +} + +/*****************************************************************************/ + +/** + Slave state: Category data. + Reads category data. +*/ + +void ec_fsm_slave_category_data(ec_fsm_t *fsm) +{ + // execute SII state machine + fsm->sii_state(fsm); + + if (fsm->sii_state == ec_fsm_sii_error) { + kfree(fsm->slave_cat_data); + fsm->slave_cat_data = NULL; + fsm->slave_state = ec_fsm_slave_finished; + EC_ERR("FSM failed to read category 0x%02X data at 0x%04X" + " on slave %i.\n", fsm->slave_cat_type, fsm->sii_offset, + fsm->slave->ring_position); + return; + } + + if (fsm->sii_state != ec_fsm_sii_finished) return; + + fsm->slave_cat_data[fsm->slave_cat_data_offset * 2] = + fsm->sii_result & 0xFF; + fsm->slave_cat_data[fsm->slave_cat_data_offset * 2 + 1] = + (fsm->sii_result >> 8) & 0xFF; + + // read second word "on the fly" + if (fsm->slave_cat_data_offset + 1 < fsm->slave_cat_words) { + fsm->slave_cat_data_offset++; + fsm->slave_cat_data[fsm->slave_cat_data_offset * 2] = + (fsm->sii_result >> 16) & 0xFF; + fsm->slave_cat_data[fsm->slave_cat_data_offset * 2 + 1] = + (fsm->sii_result >> 24) & 0xFF; + } + + fsm->slave_cat_data_offset++; + + if (fsm->slave_cat_data_offset < fsm->slave_cat_words) { + fsm->sii_offset = (fsm->slave_cat_offset + 2 + + fsm->slave_cat_data_offset); + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->slave_state = ec_fsm_slave_category_data; + fsm->slave_state(fsm); // execute state immediately + return; + } + + // category data complete + switch (fsm->slave_cat_type) + { + case 0x000A: + if (ec_slave_fetch_strings(fsm->slave, fsm->slave_cat_data)) + goto out_free; + break; + case 0x001E: + if (ec_slave_fetch_general(fsm->slave, fsm->slave_cat_data)) + goto out_free; + break; + case 0x0028: + break; + case 0x0029: + if (ec_slave_fetch_sync(fsm->slave, fsm->slave_cat_data, + fsm->slave_cat_words)) + goto out_free; + break; + case 0x0032: + if (ec_slave_fetch_pdo(fsm->slave, fsm->slave_cat_data, + fsm->slave_cat_words, + EC_TX_PDO)) + goto out_free; + break; + case 0x0033: + if (ec_slave_fetch_pdo(fsm->slave, fsm->slave_cat_data, + fsm->slave_cat_words, + EC_RX_PDO)) + goto out_free; + break; + default: + EC_WARN("FSM: Unknown category type 0x%04X in slave %i.\n", + fsm->slave_cat_type, fsm->slave->ring_position); + } + + // start reading next category header + fsm->slave_cat_offset += 2 + fsm->slave_cat_words; + fsm->sii_offset = fsm->slave_cat_offset; + fsm->sii_state = ec_fsm_sii_start_reading; + fsm->slave_state = ec_fsm_slave_category_header; + fsm->slave_state(fsm); // execute state immediately + return; + + out_free: + kfree(fsm->slave_cat_data); + fsm->slave_cat_data = NULL; + fsm->slave_state = ec_fsm_slave_finished; +} + +/*****************************************************************************/ + +/** + Slave state: Finished. + End state of the slave state machine. +*/ + +void ec_fsm_slave_finished(ec_fsm_t *fsm) +{ +} + +/****************************************************************************** + * SII state machine + *****************************************************************************/ + +/** + Slave SII state: Start reading. + Starts reading the slave information interface. +*/ + +void ec_fsm_sii_start_reading(ec_fsm_t *fsm) +{ + ec_command_t *command = &fsm->command; + + // initiate read operation + ec_command_npwr(command, fsm->slave->station_address, 0x502, 6); + 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, fsm->sii_offset); + ec_master_queue_command(fsm->master, command); + fsm->sii_state = ec_fsm_sii_check; +} + +/*****************************************************************************/ + +/** + Slave SII state: Check. + Checks, if the SII-read-command has been sent and issues a fetch command. +*/ + +void ec_fsm_sii_check(ec_fsm_t *fsm) +{ + ec_command_t *command = &fsm->command; + + if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { + EC_ERR("FSM SII: Reception of check command failed.\n"); + fsm->sii_state = ec_fsm_sii_error; + return; + } + + ec_command_nprd(command, fsm->slave->station_address, 0x502, 10); + ec_master_queue_command(fsm->master, command); + fsm->sii_state = ec_fsm_sii_fetch; +} + +/*****************************************************************************/ + +/** + Slave SII state: Fetch. + Fetches the result of an SII-read command. +*/ + +void ec_fsm_sii_fetch(ec_fsm_t *fsm) +{ + ec_command_t *command = &fsm->command; + + if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) { + EC_ERR("FSM SII: Reception of fetch command failed.\n"); + fsm->sii_state = ec_fsm_sii_error; + return; + } + + // check "busy bit" + if (likely((EC_READ_U8(command->data + 1) & 0x81) == 0)) { + fsm->sii_result = EC_READ_U32(command->data + 6); + fsm->sii_state = ec_fsm_sii_finished; + } +} + +/*****************************************************************************/ + +/** + Slave SII state: Finished. + End state of the slave SII state machine. +*/ + +void ec_fsm_sii_finished(ec_fsm_t *fsm) +{ +} + +/*****************************************************************************/ + +/** + Slave SII state: Error. + End state of the slave SII state machine. +*/ + +void ec_fsm_sii_error(ec_fsm_t *fsm) +{ +} + +/*****************************************************************************/ diff -r 7b3126cf6dab -r b4960499098f master/fsm.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm.h Tue May 16 11:57:06 2006 +0000 @@ -0,0 +1,81 @@ +/****************************************************************************** + * + * $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; version 2 of the License. + * + * 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 + * + *****************************************************************************/ + +/** + \file + EtherCAT finite state machines. +*/ + +/*****************************************************************************/ + +#ifndef __EC_STATES__ +#define __EC_STATES__ + +#include "../include/ecrt.h" +#include "command.h" +#include "slave.h" + +/*****************************************************************************/ + +typedef struct ec_fsm ec_fsm_t; + +/*****************************************************************************/ + +/** + Finite state machine of an EtherCAT master. +*/ + +struct ec_fsm +{ + 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 */ + + void (*master_state)(ec_fsm_t *); /**< master state function */ + unsigned int master_slaves_responding; /**< number of responding slaves */ + ec_slave_state_t master_slave_states; /**< states of responding slaves */ + + void (*slave_state)(ec_fsm_t *); /**< slave state function */ + uint8_t slave_sii_num; /**< SII value iteration counter */ + uint8_t *slave_cat_data; /**< temporary memory for category data */ + uint16_t slave_cat_offset; /**< current category word offset in EEPROM */ + uint16_t slave_cat_data_offset; /**< current offset in category data */ + uint16_t slave_cat_type; /**< type of current category */ + uint16_t slave_cat_words; /**< number of words of current category */ + + void (*sii_state)(ec_fsm_t *); /**< SII state function */ + uint16_t sii_offset; /**< input: offset in SII */ + uint32_t sii_result; /**< output: read SII value (32bit) */ +}; + +/*****************************************************************************/ + +int ec_fsm_init(ec_fsm_t *, ec_master_t *); +void ec_fsm_clear(ec_fsm_t *); +void ec_fsm_reset(ec_fsm_t *); +void ec_fsm_execute(ec_fsm_t *); +int ec_fsm_idle(const ec_fsm_t *); + +/*****************************************************************************/ + +#endif diff -r 7b3126cf6dab -r b4960499098f master/master.c --- a/master/master.c Mon May 15 12:57:24 2006 +0000 +++ b/master/master.c Tue May 16 11:57:06 2006 +0000 @@ -54,11 +54,6 @@ ssize_t ec_show_master_attribute(struct kobject *, struct attribute *, char *); void ec_master_process_watch_command(ec_master_t *); -// state functions -void ec_master_state_start(ec_master_t *); -void ec_master_state_slave(ec_master_t *); -void ec_master_state_finished(ec_master_t *); - /*****************************************************************************/ /** \cond */ @@ -98,24 +93,21 @@ { EC_INFO("Initializing master %i.\n", index); + if (!(master->workqueue = create_singlethread_workqueue("EoE"))) { + EC_ERR("Failed to create master workqueue.\n"); + goto out_return; + } + + if (ec_fsm_init(&master->fsm, master)) goto out_free_wq; + master->index = index; master->device = NULL; master->reserved = 0; - INIT_LIST_HEAD(&master->slaves); INIT_LIST_HEAD(&master->command_queue); INIT_LIST_HEAD(&master->domains); INIT_LIST_HEAD(&master->eoe_slaves); - ec_command_init(&master->simple_command); - ec_command_init(&master->watch_command); - - if (!(master->workqueue = create_singlethread_workqueue("EoE"))) { - EC_ERR("Failed to create master workqueue.\n"); - goto out_return; - } - - // init freerun work INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master); // init eoe timer @@ -133,14 +125,15 @@ master->kobj.ktype = &ktype_ec_master; if (kobject_set_name(&master->kobj, "ethercat%i", index)) { EC_ERR("Failed to set kobj name.\n"); - goto out_kobj_put; + kobject_put(&master->kobj); + return -1; } ec_master_reset(master); return 0; - out_kobj_put: - kobject_put(&master->kobj); + out_free_wq: + destroy_workqueue(master->workqueue); out_return: return -1; } @@ -160,17 +153,15 @@ EC_INFO("Clearing master %i...\n", master->index); ec_master_reset(master); + ec_fsm_clear(&master->fsm); + ec_command_clear(&master->simple_command); + destroy_workqueue(master->workqueue); if (master->device) { ec_device_clear(master->device); kfree(master->device); } - ec_command_clear(&master->simple_command); - ec_command_clear(&master->watch_command); - - destroy_workqueue(master->workqueue); - EC_INFO("Master %i cleared.\n", master->index); } @@ -184,7 +175,6 @@ void ec_master_reset(ec_master_t *master /**< EtherCAT master */) { - ec_slave_t *slave, *next_s; ec_command_t *command, *next_c; ec_domain_t *domain, *next_d; ec_eoe_t *eoe, *next_eoe; @@ -201,13 +191,7 @@ // stop free-run mode ec_master_freerun_stop(master); - // remove all slaves - list_for_each_entry_safe(slave, next_s, &master->slaves, list) { - list_del(&slave->list); - kobject_del(&slave->kobj); - kobject_put(&slave->kobj); - } - master->slave_count = 0; + ec_master_clear_slaves(master); // empty command queue list_for_each_entry_safe(command, next_c, &master->command_queue, queue) { @@ -233,9 +217,6 @@ master->debug_level = 0; master->timeout = 500; // 500us - master->slaves_responding = 0; - master->slave_states = EC_SLAVE_STATE_UNKNOWN; - master->stats.timeouts = 0; master->stats.delayed = 0; master->stats.corrupted = 0; @@ -247,6 +228,26 @@ master->request_cb = NULL; master->release_cb = NULL; master->cb_data = NULL; + + ec_fsm_reset(&master->fsm); +} + +/*****************************************************************************/ + +/** + Clears all slaves. +*/ + +void ec_master_clear_slaves(ec_master_t *master) +{ + ec_slave_t *slave, *next_slave; + + list_for_each_entry_safe(slave, next_slave, &master->slaves, list) { + list_del(&slave->list); + kobject_del(&slave->kobj); + kobject_put(&slave->kobj); + } + master->slave_count = 0; } /*****************************************************************************/ @@ -516,10 +517,10 @@ int ec_master_bus_scan(ec_master_t *master /**< EtherCAT master */) { - ec_slave_t *slave, *next; + ec_slave_t *slave; ec_slave_ident_t *ident; + ec_command_t *command; unsigned int i; - ec_command_t *command; uint16_t coupler_index, coupler_subindex; uint16_t reverse_coupler_index, current_coupler_index; @@ -540,7 +541,8 @@ // init slaves for (i = 0; i < master->slave_count; i++) { - if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_KERNEL))) { + if (!(slave = + (ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_KERNEL))) { EC_ERR("Failed to allocate slave %i!\n", i); goto out_free; } @@ -591,7 +593,7 @@ if (!slave->type) { EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at" " position %i.\n", slave->sii_vendor_id, - slave->sii_product_code, i); + slave->sii_product_code, slave->ring_position); } else if (slave->type->special == EC_TYPE_BUS_COUPLER) { if (slave->sii_alias) @@ -609,11 +611,7 @@ return 0; out_free: - list_for_each_entry_safe(slave, next, &master->slaves, list) { - list_del(&slave->list); - kobject_del(&slave->kobj); - kobject_put(&slave->kobj); - } + ec_master_clear_slaves(master); return -1; } @@ -668,19 +666,8 @@ EC_INFO("Starting Free-Run mode.\n"); master->mode = EC_MASTER_MODE_FREERUN; - - if (master->watch_command.state == EC_CMD_INIT) { - if (ec_command_brd(&master->watch_command, 0x130, 2)) { - EC_ERR("Failed to allocate watchdog command!\n"); - return; - } - } - - master->freerun_state = ec_master_state_start; - - ecrt_master_prepare_async_io(master); /** \todo necessary? */ - - queue_delayed_work(master->workqueue, &master->freerun_work, HZ); + ec_fsm_reset(&master->fsm); + queue_delayed_work(master->workqueue, &master->freerun_work, HZ / 100); } /*****************************************************************************/ @@ -691,8 +678,6 @@ void ec_master_freerun_stop(ec_master_t *master /**< EtherCAT master */) { - ec_slave_t *slave, *next_slave; - if (master->mode != EC_MASTER_MODE_FREERUN) return; EC_INFO("Stopping Free-Run mode.\n"); @@ -701,13 +686,7 @@ flush_workqueue(master->workqueue); } - // remove slaves - list_for_each_entry_safe(slave, next_slave, &master->slaves, list) { - list_del(&slave->list); - kobject_del(&slave->kobj); - kobject_put(&slave->kobj); - } - + ec_master_clear_slaves(master); master->mode = EC_MASTER_MODE_IDLE; } @@ -720,106 +699,18 @@ void ec_master_freerun(void *data /**< master pointer */) { ec_master_t *master = (ec_master_t *) data; + unsigned long delay; ecrt_master_async_receive(master); - // watchdog command - ec_master_process_watch_command(master); - - if (master->watch_command.working_counter != master->slave_count) { - master->slave_count = master->watch_command.working_counter; - EC_INFO("Freerun: Topology change detected.\n"); - // reset freerun state machine - master->freerun_state = ec_master_state_start; - } - - ec_master_queue_command(master, &master->watch_command); - // execute freerun state machine - master->freerun_state(master); + ec_fsm_execute(&master->fsm); ecrt_master_async_send(master); - queue_delayed_work(master->workqueue, &master->freerun_work, HZ); -} - -/*****************************************************************************/ - -/** - Free-Run state: Start. - Processes new slave count. -*/ - -void ec_master_state_start(ec_master_t *master /**< EtherCAT master*/) -{ - ec_slave_t *slave, *next_slave; - unsigned int i; - - // remove slaves - list_for_each_entry_safe(slave, next_slave, &master->slaves, list) { - list_del(&slave->list); - kobject_del(&slave->kobj); - kobject_put(&slave->kobj); - } - - if (!master->slave_count) { - // no slaves present -> finish state machine. - master->freerun_state = ec_master_state_finished; - EC_INFO("Freerun: No slaves found.\n"); - return; - } - - // init slaves - for (i = 0; i < master->slave_count; i++) { - if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t), - GFP_KERNEL))) { - EC_ERR("Failed to allocate slave %i!\n", i); - master->freerun_state = ec_master_state_finished; - return; - } - - if (ec_slave_init(slave, master, i, i + 1)) { - master->freerun_state = ec_master_state_finished; - return; - } - - if (kobject_add(&slave->kobj)) { - EC_ERR("Failed to add kobject.\n"); - kobject_put(&slave->kobj); // free - master->freerun_state = ec_master_state_finished; - return; - } - - list_add_tail(&slave->list, &master->slaves); - } - - // begin scanning of slaves - master->freerun_slave = list_entry(master->slaves.next, ec_slave_t, list); - master->freerun_state = ec_master_state_slave; -} - -/*****************************************************************************/ - -/** - Free-Run state: Get Slave. - Executes the sub-statemachine of a slave. -*/ - -void ec_master_state_slave(ec_master_t *master /**< EtherCAT master*/) -{ - master->freerun_state = ec_master_state_finished; -} - -/*****************************************************************************/ - -/** - Free-Run state: Finished. - End state of the state machine. Does nothing. -*/ - -void ec_master_state_finished(ec_master_t *master /**< EtherCAT master*/) -{ - return; + delay = HZ / 100; + if (ec_fsm_idle(&master->fsm)) delay = HZ; + queue_delayed_work(master->workqueue, &master->freerun_work, delay); } /*****************************************************************************/ @@ -914,53 +805,6 @@ /*****************************************************************************/ /** - Processes the watchdog command. -*/ - -void ec_master_process_watch_command(ec_master_t *master - /**< EtherCAT master */ - ) -{ - unsigned int first; - - if (unlikely(master->watch_command.state == EC_CMD_INIT)) return; - - first = 1; - - if (master->watch_command.working_counter != master->slaves_responding || - master->watch_command.data[0] != master->slave_states) - { - master->slaves_responding = master->watch_command.working_counter; - master->slave_states = master->watch_command.data[0]; - - EC_INFO("%i slave%s responding (", master->slaves_responding, - master->slaves_responding == 1 ? "" : "s"); - - if (master->slave_states & EC_SLAVE_STATE_INIT) { - printk("INIT"); - first = 0; - } - if (master->slave_states & EC_SLAVE_STATE_PREOP) { - if (!first) printk(", "); - printk("PREOP"); - first = 0; - } - if (master->slave_states & EC_SLAVE_STATE_SAVEOP) { - if (!first) printk(", "); - printk("SAVEOP"); - first = 0; - } - if (master->slave_states & EC_SLAVE_STATE_OP) { - if (!first) printk(", "); - printk("OP"); - } - printk(")\n"); - } -} - -/*****************************************************************************/ - -/** Does the Ethernet-over-EtherCAT processing. */ @@ -1069,13 +913,6 @@ command = &master->simple_command; - if (master->watch_command.state == EC_CMD_INIT) { - if (ec_command_brd(&master->watch_command, 0x130, 2)) { - EC_ERR("Failed to allocate watchdog command!\n"); - return -1; - } - } - // allocate all domains domain_offset = 0; list_for_each_entry(domain, &master->domains, list) { @@ -1229,9 +1066,6 @@ return -1; } - master->slaves_responding = 0; - master->slave_states = EC_SLAVE_STATE_INIT; - return 0; } @@ -1443,9 +1277,8 @@ // output statistics ec_master_output_stats(master); - // watchdog command - ec_master_process_watch_command(master); - ec_master_queue_command(master, &master->watch_command); + // execute master state machine + ec_fsm_execute(&master->fsm); } /*****************************************************************************/ diff -r 7b3126cf6dab -r b4960499098f master/master.h --- a/master/master.h Mon May 15 12:57:24 2006 +0000 +++ b/master/master.h Tue May 16 11:57:06 2006 +0000 @@ -37,6 +37,7 @@ #include "device.h" #include "domain.h" +#include "fsm.h" /*****************************************************************************/ @@ -94,9 +95,6 @@ uint8_t command_index; /**< current command index */ struct list_head domains; /**< list of domains */ ec_command_t simple_command; /**< command structure for initialization */ - ec_command_t watch_command; /**< command for watching the slaves */ - unsigned int slaves_responding; /**< number of responding slaves */ - ec_slave_state_t slave_states; /**< states of the responding slaves */ int debug_level; /**< master debug level */ ec_stats_t stats; /**< cyclic statistics */ unsigned int timeout; /**< timeout in synchronous IO */ @@ -104,8 +102,7 @@ unsigned int reserved; /**< true, if the master is reserved for RT */ struct workqueue_struct *workqueue; /**< master workqueue */ struct work_struct freerun_work; /**< free run work object */ - void (*freerun_state)(ec_master_t *); /**< freerun state function */ - ec_slave_t *freerun_slave; /**< current slave in bus scan */ + ec_fsm_t fsm; /**< master state machine */ ec_master_mode_t mode; /**< master mode */ int (*request_cb)(void *); /**< lock request callback */ void (*release_cb)(void *); /**< lock release callback */ @@ -137,6 +134,7 @@ int ec_master_bus_scan(ec_master_t *); // misc. +void ec_master_clear_slaves(ec_master_t *); void ec_master_output_stats(ec_master_t *); /*****************************************************************************/ diff -r 7b3126cf6dab -r b4960499098f master/slave.c --- a/master/slave.c Mon May 15 12:57:24 2006 +0000 +++ b/master/slave.c Tue May 16 11:57:06 2006 +0000 @@ -39,11 +39,6 @@ /*****************************************************************************/ int ec_slave_fetch_categories(ec_slave_t *); -int ec_slave_fetch_strings(ec_slave_t *, const uint8_t *); -int ec_slave_fetch_general(ec_slave_t *, const uint8_t *); -int ec_slave_fetch_sync(ec_slave_t *, const uint8_t *, size_t); -int ec_slave_fetch_pdo(ec_slave_t *, const uint8_t *, size_t, ec_pdo_type_t); -int ec_slave_locate_string(ec_slave_t *, unsigned int, char **); ssize_t ec_show_slave_attribute(struct kobject *, struct attribute *, char *); /*****************************************************************************/ @@ -55,6 +50,7 @@ EC_SYSFS_READ_ATTR(vendor_name); EC_SYSFS_READ_ATTR(product_name); EC_SYSFS_READ_ATTR(product_desc); +EC_SYSFS_READ_ATTR(sii_desc); EC_SYSFS_READ_ATTR(type); static struct attribute *def_attrs[] = { @@ -63,6 +59,7 @@ &attr_vendor_name, &attr_product_name, &attr_product_desc, + &attr_sii_desc, &attr_type, NULL, }; @@ -492,6 +489,7 @@ /** Fetches data from slave's EEPROM. \return 0 in case of success, else < 0 + \todo memory allocation */ int ec_slave_fetch_categories(ec_slave_t *slave /**< EtherCAT slave */) @@ -1209,7 +1207,6 @@ /** Formats attribute data for SysFS read access. \return number of bytes to read - \ingroup RealTimeInterface */ ssize_t ec_show_slave_attribute(struct kobject *kobj, /**< slave's kobject */ @@ -1238,6 +1235,10 @@ if (slave->type) return sprintf(buffer, "%s\n", slave->type->description); } + else if (attr == &attr_sii_desc) { + if (slave->eeprom_desc) + return sprintf(buffer, "%s\n", slave->eeprom_desc); + } else if (attr == &attr_type) { if (slave->type) { if (slave->type->special == EC_TYPE_BUS_COUPLER) diff -r 7b3126cf6dab -r b4960499098f master/slave.h --- a/master/slave.h Mon May 15 12:57:24 2006 +0000 +++ b/master/slave.h Tue May 16 11:57:06 2006 +0000 @@ -282,6 +282,13 @@ // CoE int ec_slave_fetch_sdo_list(ec_slave_t *); +// state machine +int ec_slave_fetch_strings(ec_slave_t *, const uint8_t *); +int ec_slave_fetch_general(ec_slave_t *, const uint8_t *); +int ec_slave_fetch_sync(ec_slave_t *, const uint8_t *, size_t); +int ec_slave_fetch_pdo(ec_slave_t *, const uint8_t *, size_t, ec_pdo_type_t); +int ec_slave_locate_string(ec_slave_t *, unsigned int, char **); + // misc. void ec_slave_print(const ec_slave_t *, unsigned int); int ec_slave_check_crc(ec_slave_t *);