fp@1746: /****************************************************************************** fp@1746: * fp@1746: * $Id: fsm_coe.c 920 2007-09-12 10:07:55Z fp $ fp@1746: * fp@1746: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@1746: * fp@1746: * This file is part of the IgH EtherCAT Master. fp@1746: * fp@1746: * The IgH EtherCAT Master is free software; you can redistribute it fp@1746: * and/or modify it under the terms of the GNU General Public License fp@1746: * as published by the Free Software Foundation; either version 2 of the fp@1746: * License, or (at your option) any later version. fp@1746: * fp@1746: * The IgH EtherCAT Master is distributed in the hope that it will be fp@1746: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1746: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@1746: * GNU General Public License for more details. fp@1746: * fp@1746: * You should have received a copy of the GNU General Public License fp@1746: * along with the IgH EtherCAT Master; if not, write to the Free Software fp@1746: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@1746: * fp@1746: * The right to use EtherCAT Technology is granted and comes free of fp@1746: * charge under condition of compatibility of product made by fp@1746: * Licensee. People intending to distribute/sell products based on the fp@1746: * code, have to sign an agreement to guarantee that products using fp@1746: * software based on IgH EtherCAT master stay compatible with the actual fp@1746: * EtherCAT specification (which are released themselves as an open fp@1746: * standard) as the (only) precondition to have the right to use EtherCAT fp@1746: * Technology, IP and trade marks. fp@1746: * fp@1746: *****************************************************************************/ fp@1746: fp@1746: /** fp@1746: \file fp@1746: EtherCAT CoE mapping state machines. fp@1746: */ fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: #include "globals.h" fp@1746: #include "master.h" fp@1746: #include "mailbox.h" fp@1746: #include "canopen.h" fp@1746: #include "fsm_coe_map.h" fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *); fp@1746: fp@1746: void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *); fp@1746: fp@1746: void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *); fp@1746: void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *); fp@1746: fp@1746: void ec_fsm_coe_map_clear_pdos(ec_fsm_coe_map_t *); fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: Constructor. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_init( fp@1746: ec_fsm_coe_map_t *fsm, /**< finite state machine */ fp@1746: ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */ fp@1746: ) fp@1746: { fp@1746: fsm->state = NULL; fp@1746: fsm->fsm_coe = fsm_coe; fp@1746: INIT_LIST_HEAD(&fsm->pdos); fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: Destructor. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */) fp@1746: { fp@1746: ec_fsm_coe_map_clear_pdos(fsm); fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Clear FSM PDOs. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_clear_pdos( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: ec_pdo_t *pdo, *next; fp@1746: fp@1746: // free all PDOs fp@1746: list_for_each_entry_safe(pdo, next, &fsm->pdos, list) { fp@1746: list_del(&pdo->list); fp@1746: ec_pdo_clear(pdo); fp@1746: kfree(pdo); fp@1746: } fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: Starts to upload an SDO from a slave. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_start( fp@1746: ec_fsm_coe_map_t *fsm, /**< finite state machine */ fp@1746: ec_slave_t *slave /**< EtherCAT slave */ fp@1746: ) fp@1746: { fp@1746: fsm->slave = slave; fp@1746: fsm->state = ec_fsm_coe_map_state_start; fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: Executes the current state of the state machine. fp@1746: \return false, if state machine has terminated fp@1746: */ fp@1746: fp@1746: int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */) fp@1746: { fp@1746: fsm->state(fsm); fp@1746: fp@1746: return fsm->state != ec_fsm_coe_map_state_end fp@1746: && fsm->state != ec_fsm_coe_map_state_error; fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: Returns, if the state machine terminated with success. fp@1746: \return non-zero if successful. fp@1746: */ fp@1746: fp@1746: int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */) fp@1746: { fp@1746: return fsm->state == ec_fsm_coe_map_state_end; fp@1746: } fp@1746: fp@1746: /****************************************************************************** fp@1746: * state functions fp@1746: *****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Start reading mapping. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_start( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: // read mapping of first sync manager fp@1746: fsm->sync_index = 0; fp@1746: ec_fsm_coe_map_action_next_sync(fsm); fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Read mapping of next sync manager. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_action_next_sync( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: ec_slave_t *slave = fsm->slave; fp@1746: ec_sdo_entry_t *entry; fp@1746: fp@1746: for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) { fp@1746: if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) fp@1746: continue; fp@1746: fp@1746: if (slave->master->debug_level) fp@1746: EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n", fp@1746: fsm->sync_index, slave->ring_position); fp@1746: fp@1746: ec_fsm_coe_map_clear_pdos(fsm); fp@1746: fp@1746: if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { fp@1746: EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", fp@1746: fsm->sync_sdo->index, fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: ec_sdo_request_init_read(&fsm->request, entry); fp@1746: fsm->state = ec_fsm_coe_map_state_pdo_count; fp@1746: ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1746: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1746: return; fp@1746: } fp@1746: fp@1746: fsm->state = ec_fsm_coe_map_state_end; fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Count mapped PDOs. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_pdo_count( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1746: fp@1746: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1746: EC_ERR("Failed to read number of mapped PDOs from slave %u.\n", fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: fsm->sync_subindices = EC_READ_U8(fsm->request.data); fp@1746: fp@1746: if (fsm->slave->master->debug_level) fp@1746: EC_DBG(" %u PDOs mapped.\n", fsm->sync_subindices); fp@1746: fp@1746: // read first PDO fp@1746: fsm->sync_subindex = 1; fp@1746: ec_fsm_coe_map_action_next_pdo(fsm); fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Read next PDO. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_action_next_pdo( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: ec_sdo_entry_t *entry; fp@1746: fp@1746: if (fsm->sync_subindex <= fsm->sync_subindices) { fp@1746: if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, fp@1746: fsm->sync_subindex))) { fp@1746: EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n", fp@1746: fsm->sync_sdo->index, fp@1746: fsm->sync_subindex, fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: ec_sdo_request_init_read(&fsm->request, entry); fp@1746: fsm->state = ec_fsm_coe_map_state_pdo; fp@1746: ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1746: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1746: return; fp@1746: } fp@1746: fp@1746: { fp@1746: ec_sync_t *sync = fsm->slave->sii_syncs + fsm->sync_index; fp@1746: ec_pdo_t *pdo; fp@1746: fp@1746: // exchange sync manager PDO mapping fp@1746: ec_sync_clear_pdos(sync); fp@1746: list_for_each_entry(pdo, &fsm->pdos, list) { fp@1746: ec_sync_add_pdo(sync, pdo); fp@1746: } fp@1749: sync->mapping_source = EC_SYNC_MAPPING_COE; fp@1746: ec_fsm_coe_map_clear_pdos(fsm); fp@1746: fp@1746: // next sync manager fp@1746: fsm->sync_index++; fp@1746: ec_fsm_coe_map_action_next_sync(fsm); fp@1746: } fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Fetch PDO information. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_pdo( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1746: fp@1746: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1746: EC_ERR("Failed to read mapped PDO index from slave %u.\n", fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: { fp@1746: ec_sdo_entry_t *entry; fp@1746: fp@1746: if (!(fsm->pdo = (ec_pdo_t *) fp@1746: kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { fp@1746: EC_ERR("Failed to allocate PDO.\n"); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: ec_pdo_init(fsm->pdo); fp@1746: fsm->pdo->index = EC_READ_U16(fsm->request.data); fp@1746: fsm->pdo->type = fp@1746: ec_sync_get_pdo_type(fsm->slave->sii_syncs + fsm->sync_index); fp@1746: fp@1746: if (fsm->slave->master->debug_level) fp@1746: EC_DBG(" PDO 0x%04X.\n", fsm->pdo->index); fp@1746: fp@1746: if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) { fp@1746: EC_ERR("Slave %u has no SDO 0x%04X.\n", fp@1746: fsm->slave->ring_position, fsm->pdo->index); fp@1746: ec_pdo_clear(fsm->pdo); fp@1746: kfree(fsm->pdo); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: if (fsm->pdo_sdo->name && strlen(fsm->pdo_sdo->name)) { fp@1746: if (!(fsm->pdo->name = (char *) fp@1746: kmalloc(strlen(fsm->pdo_sdo->name) + 1, GFP_KERNEL))) { fp@1746: EC_ERR("Failed to allocate PDO name.\n"); fp@1746: ec_pdo_clear(fsm->pdo); fp@1746: kfree(fsm->pdo); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: memcpy(fsm->pdo->name, fsm->pdo_sdo->name, fp@1746: strlen(fsm->pdo_sdo->name) + 1); fp@1746: } else { fp@1746: fsm->pdo->name = NULL; fp@1746: } fp@1746: fp@1746: if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { fp@1746: EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", fp@1746: fsm->pdo_sdo->index, fp@1746: fsm->slave->ring_position); fp@1746: ec_pdo_clear(fsm->pdo); fp@1746: kfree(fsm->pdo); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: list_add_tail(&fsm->pdo->list, &fsm->pdos); fp@1746: fp@1746: ec_sdo_request_init_read(&fsm->request, entry); fp@1746: fsm->state = ec_fsm_coe_map_state_pdo_entry_count; fp@1746: ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1746: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1746: return; fp@1746: } fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Read number of PDO entries. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_pdo_entry_count( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1746: fp@1746: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1746: EC_ERR("Failed to read number of mapped PDO entries from slave %u.\n", fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: fsm->pdo_subindices = EC_READ_U8(fsm->request.data); fp@1746: fp@1746: if (fsm->slave->master->debug_level) fp@1746: EC_DBG(" %u PDO entries mapped.\n", fsm->pdo_subindices); fp@1746: fp@1746: // read first PDO entry fp@1746: fsm->pdo_subindex = 1; fp@1746: ec_fsm_coe_map_action_next_pdo_entry(fsm); fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Read next PDO entry. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_action_next_pdo_entry( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: ec_sdo_entry_t *entry; fp@1746: fp@1746: if (fsm->pdo_subindex <= fsm->pdo_subindices) { fp@1746: if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, fp@1746: fsm->pdo_subindex))) { fp@1746: EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n", fp@1746: fsm->pdo_sdo->index, fsm->pdo_subindex, fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: ec_sdo_request_init_read(&fsm->request, entry); fp@1746: fsm->state = ec_fsm_coe_map_state_pdo_entry; fp@1746: ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); fp@1746: ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately fp@1746: return; fp@1746: } fp@1746: fp@1746: // next PDO fp@1746: fsm->sync_subindex++; fp@1746: ec_fsm_coe_map_action_next_pdo(fsm); fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: * Read PDO entry information. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_pdo_entry( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: if (ec_fsm_coe_exec(fsm->fsm_coe)) return; fp@1746: fp@1746: if (!ec_fsm_coe_success(fsm->fsm_coe)) { fp@1746: EC_ERR("Failed to read index of mapped PDO entry from slave %u.\n", fp@1746: fsm->slave->ring_position); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: { fp@1746: uint32_t pdo_entry_info; fp@1746: ec_sdo_t *sdo; fp@1746: ec_sdo_entry_t *entry; fp@1746: ec_pdo_entry_t *pdo_entry; fp@1746: fp@1746: pdo_entry_info = EC_READ_U32(fsm->request.data); fp@1746: fp@1746: if (!(pdo_entry = (ec_pdo_entry_t *) fp@1746: kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { fp@1746: EC_ERR("Failed to allocate PDO entry.\n"); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: pdo_entry->index = pdo_entry_info >> 16; fp@1746: pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; fp@1746: pdo_entry->bit_length = pdo_entry_info & 0xFF; fp@1746: fp@1746: if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) { fp@1746: EC_ERR("Slave %u has no SDO 0x%04X.\n", fp@1746: fsm->slave->ring_position, pdo_entry->index); fp@1746: kfree(pdo_entry); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) { fp@1746: EC_ERR("Slave %u has no SDO entry 0x%04X:%u.\n", fp@1746: fsm->slave->ring_position, pdo_entry->index, fp@1746: pdo_entry->subindex); fp@1746: kfree(pdo_entry); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: fp@1746: if (entry->description && strlen(entry->description)) { fp@1746: if (!(pdo_entry->name = (char *) fp@1746: kmalloc(strlen(entry->description) + 1, GFP_KERNEL))) { fp@1746: EC_ERR("Failed to allocate PDO entry name.\n"); fp@1746: kfree(pdo_entry); fp@1746: fsm->state = ec_fsm_coe_map_state_error; fp@1746: return; fp@1746: } fp@1746: memcpy(pdo_entry->name, entry->description, strlen(entry->description) + 1); fp@1746: } else { fp@1746: pdo_entry->name = NULL; fp@1746: } fp@1746: fp@1746: if (fsm->slave->master->debug_level) { fp@1746: EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", fp@1746: pdo_entry->index, pdo_entry->name ? pdo_entry->name : "???", fp@1746: pdo_entry->bit_length); fp@1746: } fp@1746: fp@1746: list_add_tail(&pdo_entry->list, &fsm->pdo->entries); fp@1746: fp@1746: // next PDO entry fp@1746: fsm->pdo_subindex++; fp@1746: ec_fsm_coe_map_action_next_pdo_entry(fsm); fp@1746: } fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: State: ERROR. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_error( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: } fp@1746: fp@1746: /*****************************************************************************/ fp@1746: fp@1746: /** fp@1746: State: END. fp@1746: */ fp@1746: fp@1746: void ec_fsm_coe_map_state_end( fp@1746: ec_fsm_coe_map_t *fsm /**< finite state machine */ fp@1746: ) fp@1746: { fp@1746: } fp@1746: fp@1746: /*****************************************************************************/