fp@792: /****************************************************************************** fp@792: * fp@792: * $Id$ fp@792: * fp@1326: * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH fp@792: * fp@792: * This file is part of the IgH EtherCAT Master. fp@792: * fp@1326: * The IgH EtherCAT Master is free software; you can redistribute it and/or fp@1326: * modify it under the terms of the GNU General Public License version 2, as fp@1326: * published by the Free Software Foundation. fp@1326: * fp@1326: * The IgH EtherCAT Master is distributed in the hope that it will be useful, fp@1326: * but WITHOUT ANY WARRANTY; without even the implied warranty of fp@1326: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General fp@1326: * Public License for more details. fp@1326: * fp@1326: * You should have received a copy of the GNU General Public License along fp@1326: * with the IgH EtherCAT Master; if not, write to the Free Software fp@792: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@792: * fp@1363: * --- fp@1363: * fp@1363: * The license mentioned above concerns the source code only. Using the fp@1363: * EtherCAT technology and brand is only permitted in compliance with the fp@1363: * industrial property and similar rights of Beckhoff Automation GmbH. fp@792: * fp@1873: * vim: expandtab fp@1873: * fp@792: *****************************************************************************/ fp@792: fp@792: /** fp@792: \file fp@792: EtherCAT slave configuration methods. fp@792: */ fp@792: fp@792: /*****************************************************************************/ fp@792: fp@2404: #include fp@995: #include fp@792: fp@792: #include "globals.h" fp@792: #include "master.h" fp@1209: #include "voe_handler.h" fp@792: fp@792: #include "slave_config.h" fp@792: fp@792: /*****************************************************************************/ fp@792: fp@792: /** Slave configuration constructor. fp@792: * fp@792: * See ecrt_master_slave_config() for the usage of the \a alias and \a fp@792: * position parameters. fp@995: */ fp@995: void ec_slave_config_init( fp@995: ec_slave_config_t *sc, /**< Slave configuration. */ fp@792: ec_master_t *master, /**< EtherCAT master. */ fp@792: uint16_t alias, /**< Slave alias. */ fp@792: uint16_t position, /**< Slave position. */ fp@792: uint32_t vendor_id, /**< Expected vendor ID. */ fp@1010: uint32_t product_code /**< Expected product code. */ fp@792: ) fp@792: { fp@1055: unsigned int i; fp@792: fp@792: sc->master = master; fp@1392: fp@792: sc->alias = alias; fp@792: sc->position = position; fp@792: sc->vendor_id = vendor_id; fp@792: sc->product_code = product_code; fp@1509: sc->watchdog_divider = 0; // use default fp@1509: sc->watchdog_intervals = 0; // use default fp@1392: fp@792: sc->slave = NULL; fp@792: fp@1082: for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) fp@1055: ec_sync_config_init(&sc->sync_configs[i]); fp@792: fp@1392: sc->used_fmmus = 0; fp@1507: sc->dc_assign_activate = 0x0000; fp@1507: sc->dc_sync[0].cycle_time = 0x00000000; fp@1507: sc->dc_sync[1].cycle_time = 0x00000000; fp@1507: sc->dc_sync[0].shift_time = 0x00000000; fp@1507: sc->dc_sync[1].shift_time = 0x00000000; fp@1392: fp@792: INIT_LIST_HEAD(&sc->sdo_configs); fp@858: INIT_LIST_HEAD(&sc->sdo_requests); fp@1209: INIT_LIST_HEAD(&sc->voe_handlers); fp@1844: INIT_LIST_HEAD(&sc->soe_configs); fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@792: /** Slave configuration destructor. fp@792: * fp@792: * Clears and frees a slave configuration object. fp@792: */ fp@995: void ec_slave_config_clear( fp@792: ec_slave_config_t *sc /**< Slave configuration. */ fp@792: ) fp@792: { fp@1055: unsigned int i; fp@854: ec_sdo_request_t *req, *next_req; fp@1209: ec_voe_handler_t *voe, *next_voe; fp@1844: ec_soe_request_t *soe, *next_soe; fp@792: fp@995: ec_slave_config_detach(sc); fp@792: fp@1055: // Free sync managers fp@1082: for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) fp@1055: ec_sync_config_clear(&sc->sync_configs[i]); fp@792: fp@1327: // free all SDO configurations fp@854: list_for_each_entry_safe(req, next_req, &sc->sdo_configs, list) { fp@854: list_del(&req->list); fp@854: ec_sdo_request_clear(req); fp@854: kfree(req); fp@854: } fp@792: fp@1327: // free all SDO requests fp@858: list_for_each_entry_safe(req, next_req, &sc->sdo_requests, list) { fp@858: list_del(&req->list); fp@858: ec_sdo_request_clear(req); fp@858: kfree(req); fp@858: } fp@1209: fp@1209: // free all VoE handlers fp@1209: list_for_each_entry_safe(voe, next_voe, &sc->voe_handlers, list) { fp@1209: list_del(&voe->list); fp@1209: ec_voe_handler_clear(voe); fp@1209: kfree(voe); fp@1209: } fp@1844: fp@1844: // free all SoE configurations fp@1844: list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) { fp@1844: list_del(&soe->list); fp@1844: ec_soe_request_clear(soe); fp@1844: kfree(soe); fp@1844: } fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@792: /** Prepares an FMMU configuration. fp@792: * fp@792: * Configuration data for the FMMU is saved in the slave config structure and fp@792: * is written to the slave during the configuration. The FMMU configuration fp@792: * is done in a way, that the complete data range of the corresponding sync fp@792: * manager is covered. Seperate FMMUs are configured for each domain. If the fp@916: * FMMU configuration is already prepared, the function does nothing and fp@916: * returns with success. fp@916: * fp@916: * \retval >=0 Success, logical offset byte address. fp@1313: * \retval <0 Error code. fp@792: */ fp@792: int ec_slave_config_prepare_fmmu( fp@792: ec_slave_config_t *sc, /**< Slave configuration. */ fp@792: ec_domain_t *domain, /**< Domain. */ fp@1055: uint8_t sync_index, /**< Sync manager index. */ fp@1327: ec_direction_t dir /**< PDO direction. */ fp@792: ) fp@792: { fp@792: unsigned int i; fp@792: ec_fmmu_config_t *fmmu; fp@792: fp@792: // FMMU configuration already prepared? fp@792: for (i = 0; i < sc->used_fmmus; i++) { fp@792: fmmu = &sc->fmmu_configs[i]; fp@1055: if (fmmu->domain == domain && fmmu->sync_index == sync_index) fp@792: return fmmu->logical_start_address; fp@792: } fp@792: fp@792: if (sc->used_fmmus == EC_MAX_FMMUS) { fp@1921: EC_CONFIG_ERR(sc, "FMMU limit reached!\n"); fp@1313: return -EOVERFLOW; fp@792: } fp@792: fp@792: fmmu = &sc->fmmu_configs[sc->used_fmmus++]; fp@1075: fp@1075: down(&sc->master->master_sem); fp@1055: ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir); fp@1075: up(&sc->master->master_sem); fp@1075: fp@792: return fmmu->logical_start_address; fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@792: /** Attaches the configuration to the addressed slave object. fp@792: * fp@1313: * \retval 0 Success. fp@1313: * \retval <0 Error code. fp@792: */ fp@792: int ec_slave_config_attach( fp@792: ec_slave_config_t *sc /**< Slave configuration. */ fp@792: ) fp@792: { fp@1507: ec_slave_t *slave; fp@1507: fp@1507: if (sc->slave) fp@1507: return 0; // already attached fp@792: fp@927: if (!(slave = ec_master_find_slave( fp@927: sc->master, sc->alias, sc->position))) { fp@1921: EC_CONFIG_DBG(sc, 1, "Failed to find slave for configuration.\n"); fp@1313: return -ENOENT; fp@927: } fp@927: fp@1507: if (slave->config) { fp@1921: EC_CONFIG_DBG(sc, 1, "Failed to attach configuration. Slave %u" fp@1921: " already has a configuration!\n", slave->ring_position); fp@1313: return -EEXIST; fp@1025: } fp@1145: fp@2139: if ( fp@2139: #ifdef EC_IDENT_WILDCARDS fp@2139: sc->vendor_id != 0xffffffff && fp@2139: #endif fp@2139: slave->sii.vendor_id != sc->vendor_id fp@2139: ) { fp@2139: EC_CONFIG_DBG(sc, 1, "Slave %u has no matching vendor ID (0x%08X)" fp@2139: " for configuration (0x%08X).\n", fp@2139: slave->ring_position, slave->sii.vendor_id, sc->vendor_id); fp@2139: return -EINVAL; fp@2139: } fp@2139: fp@2139: if ( fp@2139: #ifdef EC_IDENT_WILDCARDS fp@2139: sc->product_code != 0xffffffff && fp@2139: #endif fp@2139: slave->sii.product_code != sc->product_code fp@2139: ) { fp@2139: EC_CONFIG_DBG(sc, 1, "Slave %u has no matching product code (0x%08X)" fp@2139: " for configuration (0x%08X).\n", fp@2139: slave->ring_position, slave->sii.product_code, fp@2139: sc->product_code); fp@1313: return -EINVAL; fp@1507: } fp@1507: fp@1507: // attach slave fp@1507: slave->config = sc; fp@1507: sc->slave = slave; fp@1145: fp@1921: EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position); fp@1507: return 0; fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@792: /** Detaches the configuration from a slave object. fp@792: */ fp@792: void ec_slave_config_detach( fp@792: ec_slave_config_t *sc /**< Slave configuration. */ fp@792: ) fp@792: { fp@792: if (sc->slave) { fp@792: sc->slave->config = NULL; fp@792: sc->slave = NULL; fp@792: } fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@1327: /** Loads the default PDO assignment from the slave object. fp@879: */ fp@1055: void ec_slave_config_load_default_sync_config(ec_slave_config_t *sc) fp@1055: { fp@1055: uint8_t sync_index; fp@1055: ec_sync_config_t *sync_config; fp@1055: const ec_sync_t *sync; fp@792: fp@792: if (!sc->slave) fp@792: return; fp@792: fp@1082: for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { fp@1055: sync_config = &sc->sync_configs[sync_index]; fp@1055: if ((sync = ec_slave_get_sync(sc->slave, sync_index))) { fp@1055: sync_config->dir = ec_sync_default_direction(sync); fp@1055: if (sync_config->dir == EC_DIR_INVALID) fp@1921: EC_SLAVE_WARN(sc->slave, fp@1921: "SM%u has an invalid direction field!\n", sync_index); fp@1055: ec_pdo_list_copy(&sync_config->pdos, &sync->pdos); fp@1055: } fp@879: } fp@879: } fp@879: fp@879: /*****************************************************************************/ fp@879: fp@1327: /** Loads the default mapping for a PDO from the slave object. fp@879: */ fp@879: void ec_slave_config_load_default_mapping( fp@842: const ec_slave_config_t *sc, fp@842: ec_pdo_t *pdo fp@842: ) fp@842: { fp@1055: unsigned int i; fp@842: const ec_sync_t *sync; fp@842: const ec_pdo_t *default_pdo; fp@842: fp@1055: if (!sc->slave) fp@842: return; fp@1055: fp@1921: EC_CONFIG_DBG(sc, 1, "Loading default mapping for PDO 0x%04X.\n", fp@1921: pdo->index); fp@1055: fp@1327: // find PDO in any sync manager (it could be reassigned later) fp@1055: for (i = 0; i < sc->slave->sii.sync_count; i++) { fp@1055: sync = &sc->slave->sii.syncs[i]; fp@1055: fp@1055: list_for_each_entry(default_pdo, &sync->pdos.list, list) { fp@1055: if (default_pdo->index != pdo->index) fp@1055: continue; fp@1055: fp@1055: if (default_pdo->name) { fp@1921: EC_CONFIG_DBG(sc, 1, "Found PDO name \"%s\".\n", fp@1921: default_pdo->name); fp@1327: fp@1327: // take PDO name from assigned one fp@1055: ec_pdo_set_name(pdo, default_pdo->name); fp@1055: } fp@1055: fp@1327: // copy entries (= default PDO mapping) fp@1055: if (ec_pdo_copy_entries(pdo, default_pdo)) fp@1055: return; fp@1055: fp@1055: if (sc->master->debug_level) { fp@1055: const ec_pdo_entry_t *entry; fp@1055: list_for_each_entry(entry, &pdo->entries, list) { fp@1921: EC_CONFIG_DBG(sc, 1, "Entry 0x%04X:%02X.\n", fp@1055: entry->index, entry->subindex); fp@1055: } fp@1055: } fp@1055: fp@842: return; fp@842: } fp@792: } fp@1055: fp@1921: EC_CONFIG_DBG(sc, 1, "No default mapping found.\n"); fp@792: } fp@792: fp@990: /*****************************************************************************/ fp@990: fp@1327: /** Get the number of SDO configurations. fp@1327: * fp@1327: * \return Number of SDO configurations. fp@1092: */ fp@990: unsigned int ec_slave_config_sdo_count( fp@990: const ec_slave_config_t *sc /**< Slave configuration. */ fp@990: ) fp@990: { fp@1507: const ec_sdo_request_t *req; fp@1507: unsigned int count = 0; fp@1507: fp@1507: list_for_each_entry(req, &sc->sdo_configs, list) { fp@1507: count++; fp@1507: } fp@1507: fp@1507: return count; fp@990: } fp@990: fp@990: /*****************************************************************************/ fp@990: fp@1327: /** Finds an SDO configuration via its position in the list. fp@990: * fp@990: * Const version. fp@990: */ fp@990: const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const( fp@990: const ec_slave_config_t *sc, /**< Slave configuration. */ fp@990: unsigned int pos /**< Position in the list. */ fp@990: ) fp@990: { fp@990: const ec_sdo_request_t *req; fp@990: fp@990: list_for_each_entry(req, &sc->sdo_configs, list) { fp@990: if (pos--) fp@990: continue; fp@990: return req; fp@990: } fp@990: fp@990: return NULL; fp@990: } fp@990: fp@1263: /*****************************************************************************/ fp@1263: fp@1966: /** Get the number of IDN configurations. fp@1966: * fp@1966: * \return Number of SDO configurations. fp@1966: */ fp@1966: unsigned int ec_slave_config_idn_count( fp@1966: const ec_slave_config_t *sc /**< Slave configuration. */ fp@1966: ) fp@1966: { fp@1966: const ec_soe_request_t *req; fp@1966: unsigned int count = 0; fp@1966: fp@1966: list_for_each_entry(req, &sc->soe_configs, list) { fp@1966: count++; fp@1966: } fp@1966: fp@1966: return count; fp@1966: } fp@1966: fp@1966: /*****************************************************************************/ fp@1966: fp@1966: /** Finds an IDN configuration via its position in the list. fp@1966: * fp@1966: * Const version. fp@1966: */ fp@1966: const ec_soe_request_t *ec_slave_config_get_idn_by_pos_const( fp@1966: const ec_slave_config_t *sc, /**< Slave configuration. */ fp@1966: unsigned int pos /**< Position in the list. */ fp@1966: ) fp@1966: { fp@1966: const ec_soe_request_t *req; fp@1966: fp@1966: list_for_each_entry(req, &sc->soe_configs, list) { fp@1966: if (pos--) fp@1966: continue; fp@1966: return req; fp@1966: } fp@1966: fp@1966: return NULL; fp@1966: } fp@1966: fp@1966: /*****************************************************************************/ fp@1966: fp@1263: /** Finds a VoE handler via its position in the list. fp@1263: */ fp@1351: ec_sdo_request_t *ec_slave_config_find_sdo_request( fp@1351: ec_slave_config_t *sc, /**< Slave configuration. */ fp@1351: unsigned int pos /**< Position in the list. */ fp@1351: ) fp@1351: { fp@1351: ec_sdo_request_t *req; fp@1351: fp@1351: list_for_each_entry(req, &sc->sdo_requests, list) { fp@1351: if (pos--) fp@1351: continue; fp@1351: return req; fp@1351: } fp@1351: fp@1351: return NULL; fp@1351: } fp@1351: fp@1351: /*****************************************************************************/ fp@1351: fp@1351: /** Finds a VoE handler via its position in the list. fp@1351: */ fp@1263: ec_voe_handler_t *ec_slave_config_find_voe_handler( fp@1263: ec_slave_config_t *sc, /**< Slave configuration. */ fp@1263: unsigned int pos /**< Position in the list. */ fp@1263: ) fp@1263: { fp@1263: ec_voe_handler_t *voe; fp@1263: fp@1263: list_for_each_entry(voe, &sc->voe_handlers, list) { fp@1263: if (pos--) fp@1263: continue; fp@1263: return voe; fp@1263: } fp@1263: fp@1263: return NULL; fp@1263: } fp@1263: fp@792: /****************************************************************************** fp@1392: * Application interface fp@792: *****************************************************************************/ fp@792: fp@1055: int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, fp@1509: ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) fp@1055: { fp@1055: ec_sync_config_t *sync_config; fp@1055: fp@1921: EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p," fp@1921: " sync_index = %u, dir = %i, watchdog_mode = %i)\n", fp@1921: sc, sync_index, dir, watchdog_mode); fp@1055: fp@1082: if (sync_index >= EC_MAX_SYNC_MANAGERS) { fp@1921: EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); fp@1313: return -ENOENT; fp@1055: } fp@1055: fp@1055: if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) { fp@1931: EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (unsigned int) dir); fp@1313: return -EINVAL; fp@1055: } fp@1055: fp@1055: sync_config = &sc->sync_configs[sync_index]; fp@1055: sync_config->dir = dir; fp@1509: sync_config->watchdog_mode = watchdog_mode; fp@1055: return 0; fp@1055: } fp@1055: fp@1055: /*****************************************************************************/ fp@1055: fp@1509: void ecrt_slave_config_watchdog(ec_slave_config_t *sc, fp@1509: uint16_t divider, uint16_t intervals) fp@1509: { fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, divider = %u, intervals = %u)\n", fp@1921: __func__, sc, divider, intervals); fp@1509: fp@1509: sc->watchdog_divider = divider; fp@1509: sc->watchdog_intervals = intervals; fp@1509: } fp@1509: fp@1509: /*****************************************************************************/ fp@1509: fp@879: int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, fp@1055: uint8_t sync_index, uint16_t pdo_index) fp@1055: { fp@842: ec_pdo_t *pdo; fp@1055: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, " fp@1921: "pdo_index = 0x%04X)\n", __func__, sc, sync_index, pdo_index); fp@1055: fp@1082: if (sync_index >= EC_MAX_SYNC_MANAGERS) { fp@1921: EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); fp@1313: return -EINVAL; fp@1055: } fp@1055: fp@1075: down(&sc->master->master_sem); fp@1075: fp@1313: pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index); fp@1313: if (IS_ERR(pdo)) { fp@1075: up(&sc->master->master_sem); fp@1313: return PTR_ERR(pdo); fp@1075: } fp@1055: pdo->sync_index = sync_index; fp@842: fp@879: ec_slave_config_load_default_mapping(sc, pdo); fp@1075: fp@1075: up(&sc->master->master_sem); fp@842: return 0; fp@842: } fp@842: fp@842: /*****************************************************************************/ fp@842: fp@879: void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, fp@1055: uint8_t sync_index) fp@1055: { fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u)\n", fp@1921: __func__, sc, sync_index); fp@1055: fp@1082: if (sync_index >= EC_MAX_SYNC_MANAGERS) { fp@1921: EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); fp@1055: return; fp@1055: } fp@1055: fp@1075: down(&sc->master->master_sem); fp@1055: ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); fp@1075: up(&sc->master->master_sem); fp@879: } fp@879: fp@879: /*****************************************************************************/ fp@879: fp@879: int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, fp@879: uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, fp@842: uint8_t entry_bit_length) fp@842: { fp@1055: uint8_t sync_index; fp@1055: ec_pdo_t *pdo = NULL; fp@1313: ec_pdo_entry_t *entry; fp@1313: int retval = 0; fp@842: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, " fp@1921: "pdo_index = 0x%04X, entry_index = 0x%04X, " fp@1921: "entry_subindex = 0x%02X, entry_bit_length = %u)\n", fp@1921: __func__, sc, pdo_index, entry_index, entry_subindex, fp@1921: entry_bit_length); fp@1055: fp@1082: for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) fp@1055: if ((pdo = ec_pdo_list_find_pdo( fp@1055: &sc->sync_configs[sync_index].pdos, pdo_index))) fp@842: break; fp@842: fp@1075: if (pdo) { fp@1075: down(&sc->master->master_sem); fp@1313: entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex, fp@1313: entry_bit_length); fp@1075: up(&sc->master->master_sem); fp@1313: if (IS_ERR(entry)) fp@1313: retval = PTR_ERR(entry); fp@1075: } else { fp@1921: EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index); fp@1313: retval = -ENOENT; fp@1075: } fp@1075: fp@1075: return retval; fp@842: } fp@842: fp@842: /*****************************************************************************/ fp@842: fp@879: void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, fp@879: uint16_t pdo_index) fp@879: { fp@1055: uint8_t sync_index; fp@1055: ec_pdo_t *pdo = NULL; fp@879: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n", fp@1921: __func__, sc, pdo_index); fp@1055: fp@1082: for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) fp@1055: if ((pdo = ec_pdo_list_find_pdo( fp@1055: &sc->sync_configs[sync_index].pdos, pdo_index))) fp@879: break; fp@879: fp@1075: if (pdo) { fp@1075: down(&sc->master->master_sem); fp@1075: ec_pdo_clear_entries(pdo); fp@1075: up(&sc->master->master_sem); fp@1075: } else { fp@1921: EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index); fp@1075: } fp@879: } fp@879: fp@879: /*****************************************************************************/ fp@879: fp@1082: int ecrt_slave_config_pdos(ec_slave_config_t *sc, fp@1055: unsigned int n_syncs, const ec_sync_info_t syncs[]) fp@1055: { fp@1313: int ret; fp@1055: unsigned int i, j, k; fp@1055: const ec_sync_info_t *sync_info; fp@1055: const ec_pdo_info_t *pdo_info; fp@1055: const ec_pdo_entry_info_t *entry_info; fp@1055: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, n_syncs = %u, syncs = 0x%p)\n", fp@1921: __func__, sc, n_syncs, syncs); fp@1055: fp@1055: if (!syncs) ha@1054: return 0; ha@1054: fp@1055: for (i = 0; i < n_syncs; i++) { fp@1055: sync_info = &syncs[i]; fp@1055: fp@1097: if (sync_info->index == (uint8_t) EC_END) fp@878: break; fp@878: fp@1082: if (sync_info->index >= EC_MAX_SYNC_MANAGERS) { fp@1921: EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", fp@1921: sync_info->index); fp@1313: return -ENOENT; fp@879: } fp@879: fp@1509: ret = ecrt_slave_config_sync_manager(sc, sync_info->index, fp@1509: sync_info->dir, sync_info->watchdog_mode); fp@1313: if (ret) fp@1313: return ret; fp@879: fp@1055: if (sync_info->n_pdos && sync_info->pdos) { fp@1055: ecrt_slave_config_pdo_assign_clear(sc, sync_info->index); fp@1055: fp@1055: for (j = 0; j < sync_info->n_pdos; j++) { fp@1055: pdo_info = &sync_info->pdos[j]; fp@1055: fp@1313: ret = ecrt_slave_config_pdo_assign_add( fp@1313: sc, sync_info->index, pdo_info->index); fp@1313: if (ret) fp@1313: return ret; fp@1055: fp@1055: if (pdo_info->n_entries && pdo_info->entries) { fp@1055: ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index); fp@1055: fp@1055: for (k = 0; k < pdo_info->n_entries; k++) { fp@1055: entry_info = &pdo_info->entries[k]; fp@1055: fp@1313: ret = ecrt_slave_config_pdo_mapping_add(sc, fp@1313: pdo_info->index, entry_info->index, fp@1313: entry_info->subindex, fp@1313: entry_info->bit_length); fp@1313: if (ret) fp@1313: return ret; fp@1055: } fp@1055: } fp@842: } fp@842: } fp@842: } fp@842: fp@792: return 0; fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@807: int ecrt_slave_config_reg_pdo_entry( ha@923: ec_slave_config_t *sc, ha@923: uint16_t index, ha@923: uint8_t subindex, ha@923: ec_domain_t *domain, fp@925: unsigned int *bit_position fp@916: ) fp@916: { fp@1055: uint8_t sync_index; fp@1055: const ec_sync_config_t *sync_config; fp@925: unsigned int bit_offset, bit_pos; fp@807: ec_pdo_t *pdo; fp@807: ec_pdo_entry_t *entry; ha@923: int sync_offset; fp@807: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " fp@1921: "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n", fp@1921: __func__, sc, index, subindex, domain, bit_position); fp@1055: fp@1082: for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { fp@1055: sync_config = &sc->sync_configs[sync_index]; fp@807: bit_offset = 0; fp@1055: fp@1055: list_for_each_entry(pdo, &sync_config->pdos.list, list) { fp@807: list_for_each_entry(entry, &pdo->entries, list) { fp@807: if (entry->index != index || entry->subindex != subindex) { fp@807: bit_offset += entry->bit_length; fp@807: } else { fp@1055: bit_pos = bit_offset % 8; fp@1055: if (bit_position) { fp@1055: *bit_position = bit_pos; fp@1055: } else if (bit_pos) { fp@1921: EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does" fp@1921: " not byte-align.\n", index, subindex); fp@1313: return -EFAULT; fp@1055: } fp@1055: fp@1075: sync_offset = ec_slave_config_prepare_fmmu( fp@1075: sc, domain, sync_index, sync_config->dir); fp@1075: if (sync_offset < 0) fp@1313: return sync_offset; fp@1075: fp@1055: return sync_offset + bit_offset / 8; fp@807: } fp@807: } fp@807: } fp@807: } fp@807: fp@1921: EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X is not mapped.\n", fp@1921: index, subindex); fp@1313: return -ENOENT; ha@924: } ha@923: fp@1392: /*****************************************************************************/ fp@1392: fp@1448: void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate, fp@1438: uint32_t sync0_cycle_time, uint32_t sync0_shift_time, fp@1438: uint32_t sync1_cycle_time, uint32_t sync1_shift_time) fp@1438: { fp@1933: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, assign_activate = 0x%04X," fp@1933: " sync0_cycle = %u, sync0_shift = %u," fp@1933: " sync1_cycle = %u, sync1_shift = %u\n", fp@1933: __func__, sc, assign_activate, sync0_cycle_time, sync0_shift_time, fp@1933: sync1_cycle_time, sync1_shift_time); fp@1933: fp@1507: sc->dc_assign_activate = assign_activate; fp@1507: sc->dc_sync[0].cycle_time = sync0_cycle_time; fp@1507: sc->dc_sync[0].shift_time = sync0_shift_time; fp@1507: sc->dc_sync[1].cycle_time = sync1_cycle_time; fp@1507: sc->dc_sync[1].shift_time = sync1_shift_time; fp@1412: } fp@1412: fp@1412: /*****************************************************************************/ fp@1412: fp@893: int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, fp@893: uint8_t subindex, const uint8_t *data, size_t size) fp@893: { fp@893: ec_slave_t *slave = sc->slave; fp@893: ec_sdo_request_t *req; fp@1313: int ret; fp@893: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " fp@1921: "subindex = 0x%02X, data = 0x%p, size = %zu)\n", fp@1921: __func__, sc, index, subindex, data, size); fp@1115: fp@893: if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { fp@1921: EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n"); fp@893: } fp@893: fp@893: if (!(req = (ec_sdo_request_t *) fp@893: kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { fp@1921: EC_CONFIG_ERR(sc, "Failed to allocate memory for" fp@1921: " SDO configuration!\n"); fp@1313: return -ENOMEM; fp@893: } fp@893: fp@893: ec_sdo_request_init(req); fp@893: ec_sdo_request_address(req, index, subindex); fp@893: fp@1313: ret = ec_sdo_request_copy_data(req, data, size); fp@1313: if (ret < 0) { fp@893: ec_sdo_request_clear(req); fp@893: kfree(req); fp@1313: return ret; fp@893: } fp@893: fp@1075: down(&sc->master->master_sem); fp@893: list_add_tail(&req->list, &sc->sdo_configs); fp@1075: up(&sc->master->master_sem); fp@893: return 0; fp@893: } fp@893: fp@893: /*****************************************************************************/ fp@893: fp@893: int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index, fp@792: uint8_t subindex, uint8_t value) fp@792: { fp@792: uint8_t data[1]; fp@1115: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " fp@1921: "subindex = 0x%02X, value = %u)\n", fp@1931: __func__, sc, index, subindex, (unsigned int) value); fp@1115: fp@792: EC_WRITE_U8(data, value); fp@893: return ecrt_slave_config_sdo(sc, index, subindex, data, 1); fp@893: } fp@893: fp@893: /*****************************************************************************/ fp@893: fp@893: int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index, fp@792: uint8_t subindex, uint16_t value) fp@792: { fp@792: uint8_t data[2]; fp@1115: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " fp@1921: "subindex = 0x%02X, value = %u)\n", fp@1921: __func__, sc, index, subindex, value); fp@1115: fp@792: EC_WRITE_U16(data, value); fp@893: return ecrt_slave_config_sdo(sc, index, subindex, data, 2); fp@893: } fp@893: fp@893: /*****************************************************************************/ fp@893: fp@893: int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index, fp@792: uint8_t subindex, uint32_t value) fp@792: { fp@792: uint8_t data[4]; fp@1115: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " fp@1921: "subindex = 0x%02X, value = %u)\n", fp@1921: __func__, sc, index, subindex, value); fp@1115: fp@792: EC_WRITE_U32(data, value); fp@893: return ecrt_slave_config_sdo(sc, index, subindex, data, 4); fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@1526: int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index, fp@1526: const uint8_t *data, size_t size) fp@1526: { fp@1526: ec_slave_t *slave = sc->slave; fp@1526: ec_sdo_request_t *req; fp@1526: int ret; fp@1526: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " fp@1921: "data = 0x%p, size = %zu)\n", __func__, sc, index, data, size); fp@1526: fp@1526: if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { fp@1921: EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n"); fp@1526: } fp@1526: fp@1526: if (!(req = (ec_sdo_request_t *) fp@1526: kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { fp@1921: EC_CONFIG_ERR(sc, "Failed to allocate memory for" fp@1921: " SDO configuration!\n"); fp@1526: return -ENOMEM; fp@1526: } fp@1526: fp@1526: ec_sdo_request_init(req); fp@1526: ec_sdo_request_address(req, index, 0); fp@1526: req->complete_access = 1; fp@1526: fp@1526: ret = ec_sdo_request_copy_data(req, data, size); fp@1526: if (ret < 0) { fp@1526: ec_sdo_request_clear(req); fp@1526: kfree(req); fp@1526: return ret; fp@1526: } fp@1526: fp@1526: down(&sc->master->master_sem); fp@1526: list_add_tail(&req->list, &sc->sdo_configs); fp@1526: up(&sc->master->master_sem); fp@1526: return 0; fp@1526: } fp@1526: fp@1526: /*****************************************************************************/ fp@1526: fp@1332: /** Same as ecrt_slave_config_create_sdo_request(), but with ERR_PTR() return fp@1332: * value. fp@1332: */ fp@1312: ec_sdo_request_t *ecrt_slave_config_create_sdo_request_err( fp@1312: ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size) fp@858: { fp@858: ec_sdo_request_t *req; fp@1312: int ret; fp@858: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, " fp@1921: "index = 0x%04X, subindex = 0x%02X, size = %zu)\n", fp@1921: __func__, sc, index, subindex, size); fp@1181: fp@858: if (!(req = (ec_sdo_request_t *) fp@858: kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { fp@1921: EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n"); fp@1312: return ERR_PTR(-ENOMEM); fp@858: } fp@858: fp@858: ec_sdo_request_init(req); fp@858: ec_sdo_request_address(req, index, subindex); fp@858: fp@1312: ret = ec_sdo_request_alloc(req, size); fp@1312: if (ret < 0) { fp@858: ec_sdo_request_clear(req); fp@858: kfree(req); fp@1312: return ERR_PTR(ret); fp@858: } fp@859: fp@859: // prepare data for optional writing fp@859: memset(req->data, 0x00, size); fp@859: req->data_size = size; fp@858: fp@1075: down(&sc->master->master_sem); fp@858: list_add_tail(&req->list, &sc->sdo_requests); fp@1075: up(&sc->master->master_sem); fp@1075: fp@858: return req; fp@858: } fp@858: fp@858: /*****************************************************************************/ fp@858: fp@1312: ec_sdo_request_t *ecrt_slave_config_create_sdo_request( fp@1312: ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size) fp@1312: { fp@1312: ec_sdo_request_t *s = ecrt_slave_config_create_sdo_request_err(sc, index, fp@1312: subindex, size); fp@1312: return IS_ERR(s) ? NULL : s; fp@1312: } fp@1312: fp@1312: /*****************************************************************************/ fp@1312: fp@1332: /** Same as ecrt_slave_config_create_voe_handler(), but with ERR_PTR() return fp@1332: * value. fp@1332: */ fp@1312: ec_voe_handler_t *ecrt_slave_config_create_voe_handler_err( fp@1312: ec_slave_config_t *sc, size_t size) fp@1209: { fp@1209: ec_voe_handler_t *voe; fp@1312: int ret; fp@1209: fp@1921: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n", __func__, sc, size); fp@1209: fp@1209: if (!(voe = (ec_voe_handler_t *) fp@1209: kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) { fp@1921: EC_CONFIG_ERR(sc, "Failed to allocate VoE request memory!\n"); fp@1312: return ERR_PTR(-ENOMEM); fp@1312: } fp@1312: fp@1312: ret = ec_voe_handler_init(voe, sc, size); fp@1312: if (ret < 0) { fp@1209: kfree(voe); fp@1312: return ERR_PTR(ret); fp@1209: } fp@1209: fp@1209: down(&sc->master->master_sem); fp@1209: list_add_tail(&voe->list, &sc->voe_handlers); fp@1209: up(&sc->master->master_sem); fp@1209: fp@1209: return voe; fp@1209: } fp@1209: fp@1209: /*****************************************************************************/ fp@1209: fp@1312: ec_voe_handler_t *ecrt_slave_config_create_voe_handler( fp@1312: ec_slave_config_t *sc, size_t size) fp@1312: { fp@1312: ec_voe_handler_t *voe = ecrt_slave_config_create_voe_handler_err(sc, fp@1312: size); fp@1312: return IS_ERR(voe) ? NULL : voe; fp@1312: } fp@1312: fp@1312: /*****************************************************************************/ fp@1312: fp@1022: void ecrt_slave_config_state(const ec_slave_config_t *sc, fp@1022: ec_slave_config_state_t *state) fp@1022: { fp@1022: state->online = sc->slave ? 1 : 0; fp@1022: if (state->online) { fp@1022: state->operational = fp@1277: sc->slave->current_state == EC_SLAVE_STATE_OP fp@1277: && !sc->slave->force_config; fp@1022: state->al_state = sc->slave->current_state; fp@1022: } else { fp@1022: state->operational = 0; fp@1022: state->al_state = EC_SLAVE_STATE_UNKNOWN; fp@1022: } fp@1022: } fp@1022: fp@1022: /*****************************************************************************/ fp@1022: fp@1952: int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, fp@1952: uint16_t idn, ec_al_state_t state, const uint8_t *data, fp@1952: size_t size) fp@1844: { fp@1844: ec_slave_t *slave = sc->slave; fp@1844: ec_soe_request_t *req; fp@1844: int ret; fp@1844: fp@1952: EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, drive_no = %u, idn = 0x%04X, " fp@1952: "state = %u, data = 0x%p, size = %zu)\n", fp@1952: __func__, sc, drive_no, idn, state, data, size); fp@1952: fp@1952: if (drive_no > 7) { fp@1966: EC_CONFIG_ERR(sc, "Invalid drive number %u!\n", fp@1966: (unsigned int) drive_no); fp@1952: return -EINVAL; fp@1952: } fp@1944: fp@1944: if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) { fp@1944: EC_CONFIG_ERR(sc, "AL state for IDN config" fp@1944: " must be PREOP or SAFEOP!\n"); fp@1944: return -EINVAL; fp@1944: } fp@1844: fp@1844: if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { fp@1921: EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n"); fp@1844: } fp@1844: fp@1844: if (!(req = (ec_soe_request_t *) fp@1844: kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) { fp@1921: EC_CONFIG_ERR(sc, "Failed to allocate memory for" fp@1921: " IDN configuration!\n"); fp@1844: return -ENOMEM; fp@1844: } fp@1844: fp@1844: ec_soe_request_init(req); fp@1952: ec_soe_request_set_drive_no(req, drive_no); fp@1844: ec_soe_request_set_idn(req, idn); fp@1944: req->al_state = state; fp@1844: fp@1844: ret = ec_soe_request_copy_data(req, data, size); fp@1844: if (ret < 0) { fp@1844: ec_soe_request_clear(req); fp@1844: kfree(req); fp@1844: return ret; fp@1844: } fp@1844: fp@1844: down(&sc->master->master_sem); fp@1844: list_add_tail(&req->list, &sc->soe_configs); fp@1844: up(&sc->master->master_sem); fp@1844: return 0; fp@1844: } fp@1844: fp@1844: /*****************************************************************************/ fp@1844: fp@792: /** \cond */ fp@792: fp@1055: EXPORT_SYMBOL(ecrt_slave_config_sync_manager); fp@1509: EXPORT_SYMBOL(ecrt_slave_config_watchdog); fp@879: EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add); fp@879: EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear); fp@879: EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add); fp@879: EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear); fp@1082: EXPORT_SYMBOL(ecrt_slave_config_pdos); fp@807: EXPORT_SYMBOL(ecrt_slave_config_reg_pdo_entry); fp@1448: EXPORT_SYMBOL(ecrt_slave_config_dc); fp@893: EXPORT_SYMBOL(ecrt_slave_config_sdo); fp@792: EXPORT_SYMBOL(ecrt_slave_config_sdo8); fp@792: EXPORT_SYMBOL(ecrt_slave_config_sdo16); fp@792: EXPORT_SYMBOL(ecrt_slave_config_sdo32); fp@1526: EXPORT_SYMBOL(ecrt_slave_config_complete_sdo); fp@858: EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request); fp@1215: EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler); fp@1022: EXPORT_SYMBOL(ecrt_slave_config_state); fp@1844: EXPORT_SYMBOL(ecrt_slave_config_idn); fp@792: fp@792: /** \endcond */ fp@792: fp@792: /*****************************************************************************/