IgH EtherCAT Master: slave_config.c Source File

00001 /******************************************************************************
00002  *
00003  *  $Id$
00004  *
00005  *  Copyright (C) 2006-2012  Florian Pose, Ingenieurgemeinschaft IgH
00006  *
00007  *  This file is part of the IgH EtherCAT Master.
00008  *
00009  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
00010  *  modify it under the terms of the GNU General Public License version 2, as
00011  *  published by the Free Software Foundation.
00012  *
00013  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
00016  *  Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License along
00019  *  with the IgH EtherCAT Master; if not, write to the Free Software
00020  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  *  ---
00023  *
00024  *  The license mentioned above concerns the source code only. Using the
00025  *  EtherCAT technology and brand is only permitted in compliance with the
00026  *  industrial property and similar rights of Beckhoff Automation GmbH.
00027  *
00028  *  vim: expandtab
00029  *
00030  *****************************************************************************/
00031 
00037 /*****************************************************************************/
00038 
00039 #include <linux/module.h>
00040 #include <linux/slab.h>
00041 
00042 #include "globals.h"
00043 #include "master.h"
00044 #include "voe_handler.h"
00045 
00046 #include "slave_config.h"
00047 
00048 /*****************************************************************************/
00049 
00055 void ec_slave_config_init(
00056         ec_slave_config_t *sc, 
00057         ec_master_t *master, 
00058         uint16_t alias, 
00059         uint16_t position, 
00060         uint32_t vendor_id, 
00061         uint32_t product_code 
00062         )
00063 {
00064     unsigned int i;
00065 
00066     sc->master = master;
00067 
00068     sc->alias = alias;
00069     sc->position = position;
00070     sc->vendor_id = vendor_id;
00071     sc->product_code = product_code;
00072     sc->watchdog_divider = 0; // use default
00073     sc->watchdog_intervals = 0; // use default
00074 
00075     sc->slave = NULL;
00076 
00077     for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
00078         ec_sync_config_init(&sc->sync_configs[i]);
00079 
00080     sc->used_fmmus = 0;
00081     sc->dc_assign_activate = 0x0000;
00082     sc->dc_sync[0].cycle_time = 0U;
00083     sc->dc_sync[1].cycle_time = 0;
00084     sc->dc_sync[0].shift_time = 0U;
00085     sc->dc_sync[1].shift_time = 0;
00086 
00087     INIT_LIST_HEAD(&sc->sdo_configs);
00088     INIT_LIST_HEAD(&sc->sdo_requests);
00089     INIT_LIST_HEAD(&sc->reg_requests);
00090     INIT_LIST_HEAD(&sc->voe_handlers);
00091     INIT_LIST_HEAD(&sc->soe_configs);
00092 
00093     ec_coe_emerg_ring_init(&sc->emerg_ring, sc);
00094 }
00095 
00096 /*****************************************************************************/
00097 
00102 void ec_slave_config_clear(
00103         ec_slave_config_t *sc 
00104         )
00105 {
00106     unsigned int i;
00107     ec_sdo_request_t *req, *next_req;
00108     ec_voe_handler_t *voe, *next_voe;
00109     ec_reg_request_t *reg, *next_reg;
00110     ec_soe_request_t *soe, *next_soe;
00111 
00112     ec_slave_config_detach(sc);
00113 
00114     // Free sync managers
00115     for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
00116         ec_sync_config_clear(&sc->sync_configs[i]);
00117 
00118     // free all SDO configurations
00119     list_for_each_entry_safe(req, next_req, &sc->sdo_configs, list) {
00120         list_del(&req->list);
00121         ec_sdo_request_clear(req);
00122         kfree(req);
00123     }
00124 
00125     // free all SDO requests
00126     list_for_each_entry_safe(req, next_req, &sc->sdo_requests, list) {
00127         list_del(&req->list);
00128         ec_sdo_request_clear(req);
00129         kfree(req);
00130     }
00131 
00132     // free all register requests
00133     list_for_each_entry_safe(reg, next_reg, &sc->reg_requests, list) {
00134         list_del(&reg->list);
00135         ec_reg_request_clear(reg);
00136         kfree(reg);
00137     }
00138 
00139     // free all VoE handlers
00140     list_for_each_entry_safe(voe, next_voe, &sc->voe_handlers, list) {
00141         list_del(&voe->list);
00142         ec_voe_handler_clear(voe);
00143         kfree(voe);
00144     }
00145 
00146     // free all SoE configurations
00147     list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) {
00148         list_del(&soe->list);
00149         ec_soe_request_clear(soe);
00150         kfree(soe);
00151     }
00152 
00153     ec_coe_emerg_ring_clear(&sc->emerg_ring);
00154 }
00155 
00156 /*****************************************************************************/
00157 
00170 int ec_slave_config_prepare_fmmu(
00171         ec_slave_config_t *sc, 
00172         ec_domain_t *domain, 
00173         uint8_t sync_index, 
00174         ec_direction_t dir 
00175         )
00176 {
00177     unsigned int i;
00178     ec_fmmu_config_t *fmmu;
00179 
00180     // FMMU configuration already prepared?
00181     for (i = 0; i < sc->used_fmmus; i++) {
00182         fmmu = &sc->fmmu_configs[i];
00183         if (fmmu->domain == domain && fmmu->sync_index == sync_index)
00184             return fmmu->logical_start_address;
00185     }
00186 
00187     if (sc->used_fmmus == EC_MAX_FMMUS) {
00188         EC_CONFIG_ERR(sc, "FMMU limit reached!\n");
00189         return -EOVERFLOW;
00190     }
00191 
00192     fmmu = &sc->fmmu_configs[sc->used_fmmus++];
00193 
00194     down(&sc->master->master_sem);
00195     ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir);
00196     up(&sc->master->master_sem);
00197 
00198     return fmmu->logical_start_address;
00199 }
00200 
00201 /*****************************************************************************/
00202 
00208 int ec_slave_config_attach(
00209         ec_slave_config_t *sc 
00210         )
00211 {
00212     ec_slave_t *slave;
00213 
00214     if (sc->slave)
00215         return 0; // already attached
00216 
00217     if (!(slave = ec_master_find_slave(
00218                     sc->master, sc->alias, sc->position))) {
00219         EC_CONFIG_DBG(sc, 1, "Failed to find slave for configuration.\n");
00220         return -ENOENT;
00221     }
00222 
00223     if (slave->config) {
00224         EC_CONFIG_DBG(sc, 1, "Failed to attach configuration. Slave %u"
00225                 " already has a configuration!\n", slave->ring_position);
00226         return -EEXIST;
00227     }
00228 
00229     if (
00230 #ifdef EC_IDENT_WILDCARDS
00231             sc->vendor_id != 0xffffffff &&
00232 #endif
00233             slave->sii.vendor_id != sc->vendor_id
00234        ) {
00235         EC_CONFIG_DBG(sc, 1, "Slave %u has no matching vendor ID (0x%08X)"
00236                 " for configuration (0x%08X).\n",
00237                 slave->ring_position, slave->sii.vendor_id, sc->vendor_id);
00238         return -EINVAL;
00239     }
00240 
00241     if (
00242 #ifdef EC_IDENT_WILDCARDS
00243             sc->product_code != 0xffffffff &&
00244 #endif
00245             slave->sii.product_code != sc->product_code
00246        ) {
00247         EC_CONFIG_DBG(sc, 1, "Slave %u has no matching product code (0x%08X)"
00248                 " for configuration (0x%08X).\n",
00249                 slave->ring_position, slave->sii.product_code,
00250                 sc->product_code);
00251         return -EINVAL;
00252     }
00253 
00254     // attach slave
00255     slave->config = sc;
00256     sc->slave = slave;
00257 
00258     EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position);
00259     return 0;
00260 }
00261 
00262 /*****************************************************************************/
00263 
00266 void ec_slave_config_detach(
00267         ec_slave_config_t *sc 
00268         )
00269 {
00270     if (sc->slave) {
00271         ec_reg_request_t *reg;
00272 
00273         sc->slave->config = NULL;
00274 
00275         // invalidate processing register request
00276         list_for_each_entry(reg, &sc->reg_requests, list) {
00277             if (sc->slave->fsm.reg_request == reg) {
00278                 sc->slave->fsm.reg_request = NULL;
00279                 break;
00280             }
00281         }
00282 
00283         sc->slave = NULL;
00284     }
00285 }
00286 
00287 /*****************************************************************************/
00288 
00291 void ec_slave_config_load_default_sync_config(ec_slave_config_t *sc)
00292 {
00293     uint8_t sync_index;
00294     ec_sync_config_t *sync_config;
00295     const ec_sync_t *sync;
00296 
00297     if (!sc->slave)
00298         return;
00299 
00300     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
00301         sync_config = &sc->sync_configs[sync_index];
00302         if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {
00303             sync_config->dir = ec_sync_default_direction(sync);
00304             if (sync_config->dir == EC_DIR_INVALID)
00305                 EC_SLAVE_WARN(sc->slave,
00306                         "SM%u has an invalid direction field!\n", sync_index);
00307             ec_pdo_list_copy(&sync_config->pdos, &sync->pdos);
00308         }
00309     }
00310 }
00311 
00312 /*****************************************************************************/
00313 
00316 void ec_slave_config_load_default_mapping(
00317         const ec_slave_config_t *sc,
00318         ec_pdo_t *pdo
00319         )
00320 {
00321     unsigned int i;
00322     const ec_sync_t *sync;
00323     const ec_pdo_t *default_pdo;
00324 
00325     if (!sc->slave)
00326         return;
00327 
00328     EC_CONFIG_DBG(sc, 1, "Loading default mapping for PDO 0x%04X.\n",
00329             pdo->index);
00330 
00331     // find PDO in any sync manager (it could be reassigned later)
00332     for (i = 0; i < sc->slave->sii.sync_count; i++) {
00333         sync = &sc->slave->sii.syncs[i];
00334 
00335         list_for_each_entry(default_pdo, &sync->pdos.list, list) {
00336             if (default_pdo->index != pdo->index)
00337                 continue;
00338 
00339             if (default_pdo->name) {
00340                 EC_CONFIG_DBG(sc, 1, "Found PDO name \"%s\".\n",
00341                         default_pdo->name);
00342 
00343                 // take PDO name from assigned one
00344                 ec_pdo_set_name(pdo, default_pdo->name);
00345             }
00346 
00347             // copy entries (= default PDO mapping)
00348             if (ec_pdo_copy_entries(pdo, default_pdo))
00349                 return;
00350 
00351             if (sc->master->debug_level) {
00352                 const ec_pdo_entry_t *entry;
00353                 list_for_each_entry(entry, &pdo->entries, list) {
00354                     EC_CONFIG_DBG(sc, 1, "Entry 0x%04X:%02X.\n",
00355                             entry->index, entry->subindex);
00356                 }
00357             }
00358 
00359             return;
00360         }
00361     }
00362 
00363     EC_CONFIG_DBG(sc, 1, "No default mapping found.\n");
00364 }
00365 
00366 /*****************************************************************************/
00367 
00372 unsigned int ec_slave_config_sdo_count(
00373         const ec_slave_config_t *sc 
00374         )
00375 {
00376     const ec_sdo_request_t *req;
00377     unsigned int count = 0;
00378 
00379     list_for_each_entry(req, &sc->sdo_configs, list) {
00380         count++;
00381     }
00382 
00383     return count;
00384 }
00385 
00386 /*****************************************************************************/
00387 
00394 const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const(
00395         const ec_slave_config_t *sc, 
00396         unsigned int pos 
00397         )
00398 {
00399     const ec_sdo_request_t *req;
00400 
00401     list_for_each_entry(req, &sc->sdo_configs, list) {
00402         if (pos--)
00403             continue;
00404         return req;
00405     }
00406 
00407     return NULL;
00408 }
00409 
00410 /*****************************************************************************/
00411 
00416 unsigned int ec_slave_config_idn_count(
00417         const ec_slave_config_t *sc 
00418         )
00419 {
00420     const ec_soe_request_t *req;
00421     unsigned int count = 0;
00422 
00423     list_for_each_entry(req, &sc->soe_configs, list) {
00424         count++;
00425     }
00426 
00427     return count;
00428 }
00429 
00430 /*****************************************************************************/
00431 
00438 const ec_soe_request_t *ec_slave_config_get_idn_by_pos_const(
00439         const ec_slave_config_t *sc, 
00440         unsigned int pos 
00441         )
00442 {
00443     const ec_soe_request_t *req;
00444 
00445     list_for_each_entry(req, &sc->soe_configs, list) {
00446         if (pos--)
00447             continue;
00448         return req;
00449     }
00450 
00451     return NULL;
00452 }
00453 
00454 /*****************************************************************************/
00455 
00460 ec_sdo_request_t *ec_slave_config_find_sdo_request(
00461         ec_slave_config_t *sc, 
00462         unsigned int pos 
00463         )
00464 {
00465     ec_sdo_request_t *req;
00466 
00467     list_for_each_entry(req, &sc->sdo_requests, list) {
00468         if (pos--)
00469             continue;
00470         return req;
00471     }
00472 
00473     return NULL;
00474 }
00475 
00476 /*****************************************************************************/
00477 
00482 ec_reg_request_t *ec_slave_config_find_reg_request(
00483         ec_slave_config_t *sc, 
00484         unsigned int pos 
00485         )
00486 {
00487     ec_reg_request_t *reg;
00488 
00489     list_for_each_entry(reg, &sc->reg_requests, list) {
00490         if (pos--)
00491             continue;
00492         return reg;
00493     }
00494 
00495     return NULL;
00496 }
00497 
00498 /*****************************************************************************/
00499 
00504 ec_voe_handler_t *ec_slave_config_find_voe_handler(
00505         ec_slave_config_t *sc, 
00506         unsigned int pos 
00507         )
00508 {
00509     ec_voe_handler_t *voe;
00510 
00511     list_for_each_entry(voe, &sc->voe_handlers, list) {
00512         if (pos--)
00513             continue;
00514         return voe;
00515     }
00516 
00517     return NULL;
00518 }
00519 
00520 /******************************************************************************
00521  *  Application interface
00522  *****************************************************************************/
00523 
00524 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
00525         ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
00526 {
00527     ec_sync_config_t *sync_config;
00528 
00529     EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p,"
00530             " sync_index = %u, dir = %i, watchdog_mode = %i)\n",
00531             sc, sync_index, dir, watchdog_mode);
00532 
00533     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
00534         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
00535         return -ENOENT;
00536     }
00537 
00538     if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) {
00539         EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (unsigned int) dir);
00540         return -EINVAL;
00541     }
00542 
00543     sync_config = &sc->sync_configs[sync_index];
00544     sync_config->dir = dir;
00545     sync_config->watchdog_mode = watchdog_mode;
00546     return 0;
00547 }
00548 
00549 /*****************************************************************************/
00550 
00551 void ecrt_slave_config_watchdog(ec_slave_config_t *sc,
00552         uint16_t divider, uint16_t intervals)
00553 {
00554     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, divider = %u, intervals = %u)\n",
00555             __func__, sc, divider, intervals);
00556 
00557     sc->watchdog_divider = divider;
00558     sc->watchdog_intervals = intervals;
00559 }
00560 
00561 /*****************************************************************************/
00562 
00563 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
00564         uint8_t sync_index, uint16_t pdo_index)
00565 {
00566     ec_pdo_t *pdo;
00567 
00568     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, "
00569             "pdo_index = 0x%04X)\n", __func__, sc, sync_index, pdo_index);
00570 
00571     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
00572         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
00573         return -EINVAL;
00574     }
00575 
00576     down(&sc->master->master_sem);
00577 
00578     pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index);
00579     if (IS_ERR(pdo)) {
00580         up(&sc->master->master_sem);
00581         return PTR_ERR(pdo);
00582     }
00583     pdo->sync_index = sync_index;
00584 
00585     ec_slave_config_load_default_mapping(sc, pdo);
00586 
00587     up(&sc->master->master_sem);
00588     return 0;
00589 }
00590 
00591 /*****************************************************************************/
00592 
00593 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,
00594         uint8_t sync_index)
00595 {
00596     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u)\n",
00597             __func__, sc, sync_index);
00598 
00599     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
00600         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
00601         return;
00602     }
00603 
00604     down(&sc->master->master_sem);
00605     ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
00606     up(&sc->master->master_sem);
00607 }
00608 
00609 /*****************************************************************************/
00610 
00611 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
00612         uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,
00613         uint8_t entry_bit_length)
00614 {
00615     uint8_t sync_index;
00616     ec_pdo_t *pdo = NULL;
00617     ec_pdo_entry_t *entry;
00618     int retval = 0;
00619 
00620     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
00621             "pdo_index = 0x%04X, entry_index = 0x%04X, "
00622             "entry_subindex = 0x%02X, entry_bit_length = %u)\n",
00623             __func__, sc, pdo_index, entry_index, entry_subindex,
00624             entry_bit_length);
00625 
00626     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
00627         if ((pdo = ec_pdo_list_find_pdo(
00628                         &sc->sync_configs[sync_index].pdos, pdo_index)))
00629             break;
00630 
00631     if (pdo) {
00632         down(&sc->master->master_sem);
00633         entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex,
00634                 entry_bit_length);
00635         up(&sc->master->master_sem);
00636         if (IS_ERR(entry))
00637             retval = PTR_ERR(entry);
00638     } else {
00639         EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
00640         retval = -ENOENT;
00641     }
00642 
00643     return retval;
00644 }
00645 
00646 /*****************************************************************************/
00647 
00648 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
00649         uint16_t pdo_index)
00650 {
00651     uint8_t sync_index;
00652     ec_pdo_t *pdo = NULL;
00653 
00654     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n",
00655             __func__, sc, pdo_index);
00656 
00657     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
00658         if ((pdo = ec_pdo_list_find_pdo(
00659                         &sc->sync_configs[sync_index].pdos, pdo_index)))
00660             break;
00661 
00662     if (pdo) {
00663         down(&sc->master->master_sem);
00664         ec_pdo_clear_entries(pdo);
00665         up(&sc->master->master_sem);
00666     } else {
00667         EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
00668     }
00669 }
00670 
00671 /*****************************************************************************/
00672 
00673 int ecrt_slave_config_pdos(ec_slave_config_t *sc,
00674         unsigned int n_syncs, const ec_sync_info_t syncs[])
00675 {
00676     int ret;
00677     unsigned int i, j, k;
00678     const ec_sync_info_t *sync_info;
00679     const ec_pdo_info_t *pdo_info;
00680     const ec_pdo_entry_info_t *entry_info;
00681 
00682     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, n_syncs = %u, syncs = 0x%p)\n",
00683             __func__, sc, n_syncs, syncs);
00684 
00685     if (!syncs)
00686         return 0;
00687 
00688     for (i = 0; i < n_syncs; i++) {
00689         sync_info = &syncs[i];
00690 
00691         if (sync_info->index == (uint8_t) EC_END)
00692             break;
00693 
00694         if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {
00695             EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n",
00696                     sync_info->index);
00697             return -ENOENT;
00698         }
00699 
00700         ret = ecrt_slave_config_sync_manager(sc, sync_info->index,
00701                 sync_info->dir, sync_info->watchdog_mode);
00702         if (ret)
00703             return ret;
00704 
00705         if (sync_info->n_pdos && sync_info->pdos) {
00706             ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);
00707 
00708             for (j = 0; j < sync_info->n_pdos; j++) {
00709                 pdo_info = &sync_info->pdos[j];
00710 
00711                 ret = ecrt_slave_config_pdo_assign_add(
00712                         sc, sync_info->index, pdo_info->index);
00713                 if (ret)
00714                     return ret;
00715 
00716                 if (pdo_info->n_entries && pdo_info->entries) {
00717                     ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);
00718 
00719                     for (k = 0; k < pdo_info->n_entries; k++) {
00720                         entry_info = &pdo_info->entries[k];
00721 
00722                         ret = ecrt_slave_config_pdo_mapping_add(sc,
00723                                 pdo_info->index, entry_info->index,
00724                                 entry_info->subindex,
00725                                 entry_info->bit_length);
00726                         if (ret)
00727                             return ret;
00728                     }
00729                 }
00730             }
00731         }
00732     }
00733 
00734     return 0;
00735 }
00736 
00737 /*****************************************************************************/
00738 
00739 int ecrt_slave_config_reg_pdo_entry(
00740         ec_slave_config_t *sc,
00741         uint16_t index,
00742         uint8_t subindex,
00743         ec_domain_t *domain,
00744         unsigned int *bit_position
00745         )
00746 {
00747     uint8_t sync_index;
00748     const ec_sync_config_t *sync_config;
00749     unsigned int bit_offset, bit_pos;
00750     ec_pdo_t *pdo;
00751     ec_pdo_entry_t *entry;
00752     int sync_offset;
00753 
00754     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
00755             "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n",
00756             __func__, sc, index, subindex, domain, bit_position);
00757 
00758     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
00759         sync_config = &sc->sync_configs[sync_index];
00760         bit_offset = 0;
00761 
00762         list_for_each_entry(pdo, &sync_config->pdos.list, list) {
00763             list_for_each_entry(entry, &pdo->entries, list) {
00764                 if (entry->index != index || entry->subindex != subindex) {
00765                     bit_offset += entry->bit_length;
00766                 } else {
00767                     bit_pos = bit_offset % 8;
00768                     if (bit_position) {
00769                         *bit_position = bit_pos;
00770                     } else if (bit_pos) {
00771                         EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does"
00772                                 " not byte-align.\n", index, subindex);
00773                         return -EFAULT;
00774                     }
00775 
00776                     sync_offset = ec_slave_config_prepare_fmmu(
00777                             sc, domain, sync_index, sync_config->dir);
00778                     if (sync_offset < 0)
00779                         return sync_offset;
00780 
00781                     return sync_offset + bit_offset / 8;
00782                 }
00783             }
00784         }
00785     }
00786 
00787     EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X is not mapped.\n",
00788            index, subindex);
00789     return -ENOENT;
00790 }
00791 
00792 /*****************************************************************************/
00793 
00794 int ecrt_slave_config_reg_pdo_entry_pos(
00795         ec_slave_config_t *sc,
00796         uint8_t sync_index,
00797         unsigned int pdo_pos,
00798         unsigned int entry_pos,
00799         ec_domain_t *domain,
00800         unsigned int *bit_position
00801         )
00802 {
00803     const ec_sync_config_t *sync_config;
00804     unsigned int bit_offset, pp, ep;
00805     ec_pdo_t *pdo;
00806     ec_pdo_entry_t *entry;
00807 
00808     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, pdo_pos = %u,"
00809             " entry_pos = %u, domain = 0x%p, bit_position = 0x%p)\n",
00810             __func__, sc, sync_index, pdo_pos, entry_pos,
00811             domain, bit_position);
00812 
00813     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
00814         EC_CONFIG_ERR(sc, "Invalid syncmanager position %u.\n", sync_index);
00815         return -EINVAL;
00816     }
00817 
00818     sync_config = &sc->sync_configs[sync_index];
00819     bit_offset = 0;
00820     pp = 0;
00821 
00822     list_for_each_entry(pdo, &sync_config->pdos.list, list) {
00823         ep = 0;
00824         list_for_each_entry(entry, &pdo->entries, list) {
00825             if (pp != pdo_pos || ep != entry_pos) {
00826                 bit_offset += entry->bit_length;
00827             } else {
00828                 unsigned int bit_pos = bit_offset % 8;
00829                 int sync_offset;
00830 
00831                 if (bit_position) {
00832                     *bit_position = bit_pos;
00833                 } else if (bit_pos) {
00834                     EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does"
00835                             " not byte-align.\n",
00836                             pdo->index, entry->subindex);
00837                     return -EFAULT;
00838                 }
00839 
00840                 sync_offset = ec_slave_config_prepare_fmmu(
00841                         sc, domain, sync_index, sync_config->dir);
00842                 if (sync_offset < 0)
00843                     return sync_offset;
00844 
00845                 return sync_offset + bit_offset / 8;
00846             }
00847             ep++;
00848         }
00849         pp++;
00850     }
00851 
00852     EC_CONFIG_ERR(sc, "PDO entry specification %u/%u/%u out of range.\n",
00853            sync_index, pdo_pos, entry_pos);
00854     return -ENOENT;
00855 }
00856 
00857 /*****************************************************************************/
00858 
00859 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,
00860         uint32_t sync0_cycle_time, int32_t sync0_shift_time,
00861         uint32_t sync1_cycle_time, int32_t sync1_shift_time)
00862 {
00863     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, assign_activate = 0x%04X,"
00864             " sync0_cycle = %u, sync0_shift = %i,"
00865             " sync1_cycle = %u, sync1_shift = %i\n",
00866             __func__, sc, assign_activate, sync0_cycle_time, sync0_shift_time,
00867             sync1_cycle_time, sync1_shift_time);
00868 
00869     sc->dc_assign_activate = assign_activate;
00870     sc->dc_sync[0].cycle_time = sync0_cycle_time;
00871     sc->dc_sync[0].shift_time = sync0_shift_time;
00872     sc->dc_sync[1].cycle_time = sync1_cycle_time;
00873     sc->dc_sync[1].shift_time = sync1_shift_time;
00874 }
00875 
00876 /*****************************************************************************/
00877 
00878 int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
00879         uint8_t subindex, const uint8_t *data, size_t size)
00880 {
00881     ec_slave_t *slave = sc->slave;
00882     ec_sdo_request_t *req;
00883     int ret;
00884 
00885     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
00886             "subindex = 0x%02X, data = 0x%p, size = %zu)\n",
00887             __func__, sc, index, subindex, data, size);
00888 
00889     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
00890         EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n");
00891     }
00892 
00893     if (!(req = (ec_sdo_request_t *)
00894           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
00895         EC_CONFIG_ERR(sc, "Failed to allocate memory for"
00896                 " SDO configuration!\n");
00897         return -ENOMEM;
00898     }
00899 
00900     ec_sdo_request_init(req);
00901     ecrt_sdo_request_index(req, index, subindex);
00902 
00903     ret = ec_sdo_request_copy_data(req, data, size);
00904     if (ret < 0) {
00905         ec_sdo_request_clear(req);
00906         kfree(req);
00907         return ret;
00908     }
00909 
00910     down(&sc->master->master_sem);
00911     list_add_tail(&req->list, &sc->sdo_configs);
00912     up(&sc->master->master_sem);
00913     return 0;
00914 }
00915 
00916 /*****************************************************************************/
00917 
00918 int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,
00919         uint8_t subindex, uint8_t value)
00920 {
00921     uint8_t data[1];
00922 
00923     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
00924             "subindex = 0x%02X, value = %u)\n",
00925             __func__, sc, index, subindex, (unsigned int) value);
00926 
00927     EC_WRITE_U8(data, value);
00928     return ecrt_slave_config_sdo(sc, index, subindex, data, 1);
00929 }
00930 
00931 /*****************************************************************************/
00932 
00933 int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,
00934         uint8_t subindex, uint16_t value)
00935 {
00936     uint8_t data[2];
00937 
00938     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
00939             "subindex = 0x%02X, value = %u)\n",
00940             __func__, sc, index, subindex, value);
00941 
00942     EC_WRITE_U16(data, value);
00943     return ecrt_slave_config_sdo(sc, index, subindex, data, 2);
00944 }
00945 
00946 /*****************************************************************************/
00947 
00948 int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,
00949         uint8_t subindex, uint32_t value)
00950 {
00951     uint8_t data[4];
00952 
00953     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
00954             "subindex = 0x%02X, value = %u)\n",
00955             __func__, sc, index, subindex, value);
00956 
00957     EC_WRITE_U32(data, value);
00958     return ecrt_slave_config_sdo(sc, index, subindex, data, 4);
00959 }
00960 
00961 /*****************************************************************************/
00962 
00963 int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,
00964         const uint8_t *data, size_t size)
00965 {
00966     ec_slave_t *slave = sc->slave;
00967     ec_sdo_request_t *req;
00968     int ret;
00969 
00970     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
00971             "data = 0x%p, size = %zu)\n", __func__, sc, index, data, size);
00972 
00973     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
00974         EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n");
00975     }
00976 
00977     if (!(req = (ec_sdo_request_t *)
00978           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
00979         EC_CONFIG_ERR(sc, "Failed to allocate memory for"
00980                 " SDO configuration!\n");
00981         return -ENOMEM;
00982     }
00983 
00984     ec_sdo_request_init(req);
00985     ecrt_sdo_request_index(req, index, 0);
00986     req->complete_access = 1;
00987 
00988     ret = ec_sdo_request_copy_data(req, data, size);
00989     if (ret < 0) {
00990         ec_sdo_request_clear(req);
00991         kfree(req);
00992         return ret;
00993     }
00994 
00995     down(&sc->master->master_sem);
00996     list_add_tail(&req->list, &sc->sdo_configs);
00997     up(&sc->master->master_sem);
00998     return 0;
00999 }
01000 
01001 /*****************************************************************************/
01002 
01003 int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)
01004 {
01005     return ec_coe_emerg_ring_size(&sc->emerg_ring, elements);
01006 }
01007 
01008 /*****************************************************************************/
01009 
01010 int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)
01011 {
01012     return ec_coe_emerg_ring_pop(&sc->emerg_ring, target);
01013 }
01014 
01015 /*****************************************************************************/
01016 
01017 int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc)
01018 {
01019     return ec_coe_emerg_ring_clear_ring(&sc->emerg_ring);
01020 }
01021 
01022 /*****************************************************************************/
01023 
01024 int ecrt_slave_config_emerg_overruns(ec_slave_config_t *sc)
01025 {
01026     return ec_coe_emerg_ring_overruns(&sc->emerg_ring);
01027 }
01028 
01029 /*****************************************************************************/
01030 
01034 ec_sdo_request_t *ecrt_slave_config_create_sdo_request_err(
01035         ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
01036 {
01037     ec_sdo_request_t *req;
01038     int ret;
01039 
01040     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
01041             "index = 0x%04X, subindex = 0x%02X, size = %zu)\n",
01042             __func__, sc, index, subindex, size);
01043 
01044     if (!(req = (ec_sdo_request_t *)
01045                 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
01046         EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n");
01047         return ERR_PTR(-ENOMEM);
01048     }
01049 
01050     ec_sdo_request_init(req);
01051     ecrt_sdo_request_index(req, index, subindex);
01052 
01053     ret = ec_sdo_request_alloc(req, size);
01054     if (ret < 0) {
01055         ec_sdo_request_clear(req);
01056         kfree(req);
01057         return ERR_PTR(ret);
01058     }
01059 
01060     // prepare data for optional writing
01061     memset(req->data, 0x00, size);
01062     req->data_size = size;
01063 
01064     down(&sc->master->master_sem);
01065     list_add_tail(&req->list, &sc->sdo_requests);
01066     up(&sc->master->master_sem);
01067 
01068     return req;
01069 }
01070 
01071 /*****************************************************************************/
01072 
01073 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
01074         ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
01075 {
01076     ec_sdo_request_t *s = ecrt_slave_config_create_sdo_request_err(sc, index,
01077             subindex, size);
01078     return IS_ERR(s) ? NULL : s;
01079 }
01080 
01081 /*****************************************************************************/
01082 
01086 ec_reg_request_t *ecrt_slave_config_create_reg_request_err(
01087         ec_slave_config_t *sc, size_t size)
01088 {
01089     ec_reg_request_t *reg;
01090     int ret;
01091 
01092     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n",
01093             __func__, sc, size);
01094 
01095     if (!(reg = (ec_reg_request_t *)
01096                 kmalloc(sizeof(ec_reg_request_t), GFP_KERNEL))) {
01097         EC_CONFIG_ERR(sc, "Failed to allocate register request memory!\n");
01098         return ERR_PTR(-ENOMEM);
01099     }
01100 
01101     ret = ec_reg_request_init(reg, size);
01102     if (ret) {
01103         kfree(reg);
01104         return ERR_PTR(ret);
01105     }
01106 
01107     down(&sc->master->master_sem);
01108     list_add_tail(&reg->list, &sc->reg_requests);
01109     up(&sc->master->master_sem);
01110 
01111     return reg;
01112 }
01113 
01114 /*****************************************************************************/
01115 
01116 ec_reg_request_t *ecrt_slave_config_create_reg_request(
01117         ec_slave_config_t *sc, size_t size)
01118 {
01119     ec_reg_request_t *reg =
01120         ecrt_slave_config_create_reg_request_err(sc, size);
01121     return IS_ERR(reg) ? NULL : reg;
01122 }
01123 
01124 /*****************************************************************************/
01125 
01129 ec_voe_handler_t *ecrt_slave_config_create_voe_handler_err(
01130         ec_slave_config_t *sc, size_t size)
01131 {
01132     ec_voe_handler_t *voe;
01133     int ret;
01134 
01135     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n", __func__, sc, size);
01136 
01137     if (!(voe = (ec_voe_handler_t *)
01138                 kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) {
01139         EC_CONFIG_ERR(sc, "Failed to allocate VoE request memory!\n");
01140         return ERR_PTR(-ENOMEM);
01141     }
01142 
01143     ret = ec_voe_handler_init(voe, sc, size);
01144     if (ret < 0) {
01145         kfree(voe);
01146         return ERR_PTR(ret);
01147     }
01148 
01149     down(&sc->master->master_sem);
01150     list_add_tail(&voe->list, &sc->voe_handlers);
01151     up(&sc->master->master_sem);
01152 
01153     return voe;
01154 }
01155 
01156 /*****************************************************************************/
01157 
01158 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(
01159         ec_slave_config_t *sc, size_t size)
01160 {
01161     ec_voe_handler_t *voe = ecrt_slave_config_create_voe_handler_err(sc,
01162             size);
01163     return IS_ERR(voe) ? NULL : voe;
01164 }
01165 
01166 /*****************************************************************************/
01167 
01168 void ecrt_slave_config_state(const ec_slave_config_t *sc,
01169         ec_slave_config_state_t *state)
01170 {
01171     state->online = sc->slave ? 1 : 0;
01172     if (state->online) {
01173         state->operational =
01174             sc->slave->current_state == EC_SLAVE_STATE_OP
01175             && !sc->slave->force_config;
01176         state->al_state = sc->slave->current_state;
01177     } else {
01178         state->operational = 0;
01179         state->al_state = EC_SLAVE_STATE_UNKNOWN;
01180     }
01181 }
01182 
01183 /*****************************************************************************/
01184 
01185 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
01186         uint16_t idn, ec_al_state_t state, const uint8_t *data,
01187         size_t size)
01188 {
01189     ec_slave_t *slave = sc->slave;
01190     ec_soe_request_t *req;
01191     int ret;
01192 
01193     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, drive_no = %u, idn = 0x%04X, "
01194             "state = %u, data = 0x%p, size = %zu)\n",
01195             __func__, sc, drive_no, idn, state, data, size);
01196 
01197     if (drive_no > 7) {
01198         EC_CONFIG_ERR(sc, "Invalid drive number %u!\n",
01199                 (unsigned int) drive_no);
01200         return -EINVAL;
01201     }
01202 
01203     if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) {
01204         EC_CONFIG_ERR(sc, "AL state for IDN config"
01205                 " must be PREOP or SAFEOP!\n");
01206         return -EINVAL;
01207     }
01208 
01209     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
01210         EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n");
01211     }
01212 
01213     if (!(req = (ec_soe_request_t *)
01214           kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) {
01215         EC_CONFIG_ERR(sc, "Failed to allocate memory for"
01216                 " IDN configuration!\n");
01217         return -ENOMEM;
01218     }
01219 
01220     ec_soe_request_init(req);
01221     ec_soe_request_set_drive_no(req, drive_no);
01222     ec_soe_request_set_idn(req, idn);
01223     req->al_state = state;
01224 
01225     ret = ec_soe_request_copy_data(req, data, size);
01226     if (ret < 0) {
01227         ec_soe_request_clear(req);
01228         kfree(req);
01229         return ret;
01230     }
01231 
01232     down(&sc->master->master_sem);
01233     list_add_tail(&req->list, &sc->soe_configs);
01234     up(&sc->master->master_sem);
01235     return 0;
01236 }
01237 
01238 /*****************************************************************************/
01239 
01242 EXPORT_SYMBOL(ecrt_slave_config_sync_manager);
01243 EXPORT_SYMBOL(ecrt_slave_config_watchdog);
01244 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add);
01245 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear);
01246 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add);
01247 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear);
01248 EXPORT_SYMBOL(ecrt_slave_config_pdos);
01249 EXPORT_SYMBOL(ecrt_slave_config_reg_pdo_entry);
01250 EXPORT_SYMBOL(ecrt_slave_config_dc);
01251 EXPORT_SYMBOL(ecrt_slave_config_sdo);
01252 EXPORT_SYMBOL(ecrt_slave_config_sdo8);
01253 EXPORT_SYMBOL(ecrt_slave_config_sdo16);
01254 EXPORT_SYMBOL(ecrt_slave_config_sdo32);
01255 EXPORT_SYMBOL(ecrt_slave_config_complete_sdo);
01256 EXPORT_SYMBOL(ecrt_slave_config_emerg_size);
01257 EXPORT_SYMBOL(ecrt_slave_config_emerg_pop);
01258 EXPORT_SYMBOL(ecrt_slave_config_emerg_clear);
01259 EXPORT_SYMBOL(ecrt_slave_config_emerg_overruns);
01260 EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request);
01261 EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler);
01262 EXPORT_SYMBOL(ecrt_slave_config_create_reg_request);
01263 EXPORT_SYMBOL(ecrt_slave_config_state);
01264 EXPORT_SYMBOL(ecrt_slave_config_idn);
01265 
01268 /*****************************************************************************/