fp@1174: /****************************************************************************** fp@1174: * fp@1174: * $Id$ fp@1174: * fp@1174: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@1174: * fp@1174: * This file is part of the IgH EtherCAT Master. fp@1174: * fp@1174: * The IgH EtherCAT Master is free software; you can redistribute it fp@1174: * and/or modify it under the terms of the GNU General Public License fp@1174: * as published by the Free Software Foundation; either version 2 of the fp@1174: * License, or (at your option) any later version. fp@1174: * fp@1174: * The IgH EtherCAT Master is distributed in the hope that it will be fp@1174: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1174: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@1174: * GNU General Public License for more details. fp@1174: * fp@1174: * You should have received a copy of the GNU General Public License fp@1174: * along with the IgH EtherCAT Master; if not, write to the Free Software fp@1174: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@1174: * fp@1174: * The right to use EtherCAT Technology is granted and comes free of fp@1174: * charge under condition of compatibility of product made by fp@1174: * Licensee. People intending to distribute/sell products based on the fp@1174: * code, have to sign an agreement to guarantee that products using fp@1174: * software based on IgH EtherCAT master stay compatible with the actual fp@1174: * EtherCAT specification (which are released themselves as an open fp@1174: * standard) as the (only) precondition to have the right to use EtherCAT fp@1174: * Technology, IP and trade marks. fp@1174: * fp@1174: *****************************************************************************/ fp@1174: fp@1174: /** \file fp@1174: * EtherCAT Pdo mapping state machine. fp@1174: */ fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: #include "globals.h" fp@1174: #include "master.h" fp@1174: #include "mailbox.h" fp@1174: #include "slave_config.h" fp@1174: fp@1174: #include "fsm_pdo_entry.h" fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *); fp@1174: void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *); fp@1174: void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *); fp@1174: fp@1174: void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *); fp@1174: fp@1174: void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *); fp@1174: void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *); fp@1174: void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *); fp@1174: void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *); fp@1174: fp@1174: void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *); fp@1174: void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *); fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Constructor. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_init( fp@1174: ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ fp@1174: ec_fsm_coe_t *fsm_coe /**< CoE state machine to use. */ fp@1174: ) fp@1174: { fp@1174: fsm->fsm_coe = fsm_coe; fp@1174: ec_sdo_request_init(&fsm->request); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Destructor. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_clear( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: ec_sdo_request_clear(&fsm->request); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Start reading a Pdo's entries. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_start_reading( fp@1174: ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ fp@1174: ec_slave_t *slave, /**< slave to configure */ fp@1174: ec_pdo_t *pdo /**< Pdo to read entries for. */ fp@1174: ) fp@1174: { fp@1174: fsm->slave = slave; fp@1174: fsm->target_pdo = pdo; fp@1174: fp@1174: ec_pdo_clear_entries(fsm->target_pdo); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_read_state_start; fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Start Pdo mapping state machine. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_start_configuration( fp@1174: ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ fp@1174: ec_slave_t *slave, /**< slave to configure */ fp@1174: const ec_pdo_t *pdo /**< Pdo with the desired entries. */ fp@1174: ) fp@1174: { fp@1174: fsm->slave = slave; fp@1174: fsm->source_pdo = pdo; fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_conf_state_start; fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Get running state. fp@1174: * fp@1174: * \return false, if state machine has terminated fp@1174: */ fp@1174: int ec_fsm_pdo_entry_running( fp@1174: const ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: return fsm->state != ec_fsm_pdo_entry_state_end fp@1174: && fsm->state != ec_fsm_pdo_entry_state_error; fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Executes the current state. fp@1174: * fp@1174: * \return false, if state machine has terminated fp@1174: */ fp@1174: int ec_fsm_pdo_entry_exec( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: fsm->state(fsm); fp@1174: return ec_fsm_pdo_entry_running(fsm); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Get execution result. fp@1174: * fp@1174: * \return true, if the state machine terminated gracefully fp@1174: */ fp@1174: int ec_fsm_pdo_entry_success( fp@1174: const ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: return fsm->state == ec_fsm_pdo_entry_state_end; fp@1174: } fp@1174: fp@1174: /****************************************************************************** fp@1174: * Reading state functions. fp@1174: *****************************************************************************/ fp@1174: fp@1174: /** Request reading the number of mapped Pdo entries. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_read_state_start( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, 0); fp@1174: ecrt_sdo_request_read(&fsm->request); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_read_state_count; fp@1174: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Read number of mapped Pdo entries. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_read_state_count( fp@1174: ec_fsm_pdo_entry_t *fsm /**< finite state machine */ fp@1174: ) fp@1174: { fp@1174: if (ec_fsm_coe_exec(fsm->fsm_coe)) fp@1174: return; fp@1174: fp@1174: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1174: EC_ERR("Failed to read number of mapped Pdo entries.\n"); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: if (fsm->request.data_size != sizeof(uint8_t)) { fp@1174: EC_ERR("Invalid data size %u at uploading Sdo 0x%04X:%02X.\n", fp@1174: fsm->request.data_size, fsm->request.index, fp@1174: fsm->request.subindex); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: fsm->entry_count = EC_READ_U8(fsm->request.data); fp@1174: fp@1174: if (fsm->slave->master->debug_level) fp@1174: EC_DBG("%u Pdo entries mapped.\n", fsm->entry_count); fp@1174: fp@1174: // read first Pdo entry fp@1174: fsm->entry_pos = 1; fp@1174: ec_fsm_pdo_entry_read_action_next(fsm); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Read next Pdo entry. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_read_action_next( fp@1174: ec_fsm_pdo_entry_t *fsm /**< finite state machine */ fp@1174: ) fp@1174: { fp@1174: if (fsm->entry_pos <= fsm->entry_count) { fp@1174: ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, fsm->entry_pos); fp@1174: ecrt_sdo_request_read(&fsm->request); fp@1174: fsm->state = ec_fsm_pdo_entry_read_state_entry; fp@1174: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1174: return; fp@1174: } fp@1174: fp@1174: // finished reading entries. fp@1174: fsm->state = ec_fsm_pdo_entry_state_end; fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Read Pdo entry information. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_read_state_entry( fp@1174: ec_fsm_pdo_entry_t *fsm /**< finite state machine */ fp@1174: ) fp@1174: { fp@1174: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1174: fp@1174: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1174: EC_ERR("Failed to read mapped Pdo entry.\n"); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: if (fsm->request.data_size != sizeof(uint32_t)) { fp@1174: EC_ERR("Invalid data size %u at uploading Sdo 0x%04X:%02X.\n", fp@1174: fsm->request.data_size, fsm->request.index, fp@1174: fsm->request.subindex); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: } else { fp@1174: uint32_t pdo_entry_info; fp@1174: ec_pdo_entry_t *pdo_entry; fp@1174: fp@1174: pdo_entry_info = EC_READ_U32(fsm->request.data); fp@1174: fp@1174: if (!(pdo_entry = (ec_pdo_entry_t *) fp@1174: kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { fp@1174: EC_ERR("Failed to allocate Pdo entry.\n"); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: ec_pdo_entry_init(pdo_entry); fp@1174: pdo_entry->index = pdo_entry_info >> 16; fp@1174: pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; fp@1174: pdo_entry->bit_length = pdo_entry_info & 0xFF; fp@1174: fp@1174: if (!pdo_entry->index && !pdo_entry->subindex) { fp@1174: if (ec_pdo_entry_set_name(pdo_entry, "Gap")) { fp@1174: ec_pdo_entry_clear(pdo_entry); fp@1174: kfree(pdo_entry); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: } fp@1174: fp@1174: if (fsm->slave->master->debug_level) { fp@1174: EC_DBG("Pdo entry 0x%04X:%02X, %u bit, \"%s\".\n", fp@1174: pdo_entry->index, pdo_entry->subindex, fp@1174: pdo_entry->bit_length, fp@1174: pdo_entry->name ? pdo_entry->name : "???"); fp@1174: } fp@1174: fp@1174: list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); fp@1174: fp@1174: // next Pdo entry fp@1174: fsm->entry_pos++; fp@1174: ec_fsm_pdo_entry_read_action_next(fsm); fp@1174: } fp@1174: } fp@1174: fp@1174: /****************************************************************************** fp@1174: * Configuration state functions. fp@1174: *****************************************************************************/ fp@1174: fp@1174: /** Start Pdo mapping. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_conf_state_start( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: // Pdo mapping has to be changed. Does the slave support this? fp@1174: if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) fp@1174: || (fsm->slave->sii.has_general fp@1174: && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { fp@1174: EC_WARN("Slave %u does not support changing the Pdo mapping!\n", fp@1174: fsm->slave->ring_position); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: if (ec_sdo_request_alloc(&fsm->request, 4)) { fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: // set mapped Pdo entry count to zero fp@1174: EC_WRITE_U8(fsm->request.data, 0); fp@1174: fsm->request.data_size = 1; fp@1174: ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); fp@1174: ecrt_sdo_request_write(&fsm->request); fp@1174: fp@1174: if (fsm->slave->master->debug_level) fp@1174: EC_DBG("Setting entry count to zero.\n"); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count; fp@1174: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Process next Pdo entry. fp@1174: */ fp@1174: ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry( fp@1174: const ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ fp@1174: const struct list_head *list /**< current entry list item */ fp@1174: ) fp@1174: { fp@1174: list = list->next; fp@1174: if (list == &fsm->source_pdo->entries) fp@1174: return NULL; // no next entry fp@1174: return list_entry(list, ec_pdo_entry_t, list); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Set the number of mapped entries to zero. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_conf_state_zero_entry_count( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: if (ec_fsm_coe_exec(fsm->fsm_coe)) fp@1174: return; fp@1174: fp@1174: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1174: EC_WARN("Failed to clear Pdo mapping.\n"); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: // find first entry fp@1174: if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry( fp@1174: fsm, &fsm->source_pdo->entries))) { fp@1174: fp@1174: if (fsm->slave->master->debug_level) fp@1174: EC_DBG("No entries to map.\n"); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_state_end; // finished fp@1174: return; fp@1174: } fp@1174: fp@1174: // add first entry fp@1174: fsm->entry_pos = 1; fp@1174: ec_fsm_pdo_entry_conf_action_map(fsm); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Starts to add a Pdo entry. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_conf_action_map( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: uint32_t value; fp@1174: fp@1174: if (fsm->slave->master->debug_level) fp@1174: EC_DBG("Mapping Pdo entry 0x%04X:%02X (%u bit) at position %u.\n", fp@1174: fsm->entry->index, fsm->entry->subindex, fp@1174: fsm->entry->bit_length, fsm->entry_pos); fp@1174: fp@1174: value = fsm->entry->index << 16 fp@1174: | fsm->entry->subindex << 8 | fsm->entry->bit_length; fp@1174: EC_WRITE_U32(fsm->request.data, value); fp@1174: fsm->request.data_size = 4; fp@1174: ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, fsm->entry_pos); fp@1174: ecrt_sdo_request_write(&fsm->request); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_conf_state_map_entry; fp@1174: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Add a Pdo entry. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_conf_state_map_entry( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1174: fp@1174: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1174: EC_WARN("Failed to map Pdo entry 0x%04X:%02X (%u bit) to " fp@1174: "position %u.\n", fsm->entry->index, fsm->entry->subindex, fp@1174: fsm->entry->bit_length, fsm->entry_pos); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: // find next entry fp@1174: if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry( fp@1174: fsm, &fsm->entry->list))) { fp@1174: fp@1174: // No more entries to add. Write entry count. fp@1174: EC_WRITE_U8(fsm->request.data, fsm->entry_pos); fp@1174: fsm->request.data_size = 1; fp@1174: ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); fp@1174: ecrt_sdo_request_write(&fsm->request); fp@1174: fp@1174: if (fsm->slave->master->debug_level) fp@1174: EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_pos); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count; fp@1174: ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1174: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1174: return; fp@1174: } fp@1174: fp@1174: // add next entry fp@1174: fsm->entry_pos++; fp@1174: ec_fsm_pdo_entry_conf_action_map(fsm); fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** Set the number of entries. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_conf_state_set_entry_count( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1174: fp@1174: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1174: EC_ERR("Failed to set number of entries.\n"); fp@1174: fsm->state = ec_fsm_pdo_entry_state_error; fp@1174: return; fp@1174: } fp@1174: fp@1174: if (fsm->slave->master->debug_level) fp@1174: EC_DBG("Successfully configured mapping for Pdo 0x%04X.\n", fp@1174: fsm->source_pdo->index); fp@1174: fp@1174: fsm->state = ec_fsm_pdo_entry_state_end; // finished fp@1174: } fp@1174: fp@1174: /****************************************************************************** fp@1174: * Common state functions fp@1174: *****************************************************************************/ fp@1174: fp@1174: /** State: ERROR. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_state_error( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: } fp@1174: fp@1174: /*****************************************************************************/ fp@1174: fp@1174: /** State: END. fp@1174: */ fp@1174: void ec_fsm_pdo_entry_state_end( fp@1174: ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ fp@1174: ) fp@1174: { fp@1174: } fp@1174: fp@1174: /*****************************************************************************/