master/slave_config.c
changeset 2589 2b9c78543663
parent 2411 01370d59233d
child 2609 777d1a8b3a27
child 2634 f859d567f94e
equal deleted inserted replaced
2415:af21f0bdc7c9 2589:2b9c78543663
     1 /******************************************************************************
     1 /******************************************************************************
     2  *
     2  *
     3  *  $Id$
     3  *  $Id$
     4  *
     4  *
     5  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
     5  *  Copyright (C) 2006-2012  Florian Pose, Ingenieurgemeinschaft IgH
     6  *
     6  *
     7  *  This file is part of the IgH EtherCAT Master.
     7  *  This file is part of the IgH EtherCAT Master.
     8  *
     8  *
     9  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
     9  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
    10  *  modify it under the terms of the GNU General Public License version 2, as
    10  *  modify it under the terms of the GNU General Public License version 2, as
    69     sc->position = position;
    69     sc->position = position;
    70     sc->vendor_id = vendor_id;
    70     sc->vendor_id = vendor_id;
    71     sc->product_code = product_code;
    71     sc->product_code = product_code;
    72     sc->watchdog_divider = 0; // use default
    72     sc->watchdog_divider = 0; // use default
    73     sc->watchdog_intervals = 0; // use default
    73     sc->watchdog_intervals = 0; // use default
    74     sc->allow_overlapping_pdos = 0; // default not allowed
    74 
    75     sc->slave = NULL;
    75     sc->slave = NULL;
    76 
    76 
    77     for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
    77     for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
    78         ec_sync_config_init(&sc->sync_configs[i]);
    78         ec_sync_config_init(&sc->sync_configs[i]);
    79 
    79 
    80     sc->used_fmmus = 0;
    80     sc->used_fmmus = 0;
    81     sc->dc_assign_activate = 0x0000;
    81     sc->dc_assign_activate = 0x0000;
    82     sc->dc_sync[0].cycle_time = 0x00000000;
    82     sc->dc_sync[0].cycle_time = 0U;
    83     sc->dc_sync[1].cycle_time = 0x00000000;
    83     sc->dc_sync[1].cycle_time = 0;
    84     sc->dc_sync[0].shift_time = 0x00000000;
    84     sc->dc_sync[0].shift_time = 0U;
    85     sc->dc_sync[1].shift_time = 0x00000000;
    85     sc->dc_sync[1].shift_time = 0;
    86 
    86 
    87     INIT_LIST_HEAD(&sc->sdo_configs);
    87     INIT_LIST_HEAD(&sc->sdo_configs);
    88     INIT_LIST_HEAD(&sc->sdo_requests);
    88     INIT_LIST_HEAD(&sc->sdo_requests);
       
    89     INIT_LIST_HEAD(&sc->reg_requests);
    89     INIT_LIST_HEAD(&sc->voe_handlers);
    90     INIT_LIST_HEAD(&sc->voe_handlers);
    90     INIT_LIST_HEAD(&sc->soe_configs);
    91     INIT_LIST_HEAD(&sc->soe_configs);
       
    92 
       
    93     ec_coe_emerg_ring_init(&sc->emerg_ring, sc);
    91 }
    94 }
    92 
    95 
    93 /*****************************************************************************/
    96 /*****************************************************************************/
    94 
    97 
    95 /** Slave configuration destructor.
    98 /** Slave configuration destructor.
   101         )
   104         )
   102 {
   105 {
   103     unsigned int i;
   106     unsigned int i;
   104     ec_sdo_request_t *req, *next_req;
   107     ec_sdo_request_t *req, *next_req;
   105     ec_voe_handler_t *voe, *next_voe;
   108     ec_voe_handler_t *voe, *next_voe;
       
   109     ec_reg_request_t *reg, *next_reg;
   106     ec_soe_request_t *soe, *next_soe;
   110     ec_soe_request_t *soe, *next_soe;
   107 
   111 
   108     ec_slave_config_detach(sc);
   112     ec_slave_config_detach(sc);
   109 
   113 
   110     // Free sync managers
   114     // Free sync managers
   123         list_del(&req->list);
   127         list_del(&req->list);
   124         ec_sdo_request_clear(req);
   128         ec_sdo_request_clear(req);
   125         kfree(req);
   129         kfree(req);
   126     }
   130     }
   127 
   131 
       
   132     // free all register requests
       
   133     list_for_each_entry_safe(reg, next_reg, &sc->reg_requests, list) {
       
   134         list_del(&reg->list);
       
   135         ec_reg_request_clear(reg);
       
   136         kfree(reg);
       
   137     }
       
   138 
   128     // free all VoE handlers
   139     // free all VoE handlers
   129     list_for_each_entry_safe(voe, next_voe, &sc->voe_handlers, list) {
   140     list_for_each_entry_safe(voe, next_voe, &sc->voe_handlers, list) {
   130         list_del(&voe->list);
   141         list_del(&voe->list);
   131         ec_voe_handler_clear(voe);
   142         ec_voe_handler_clear(voe);
   132         kfree(voe);
   143         kfree(voe);
   136     list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) {
   147     list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) {
   137         list_del(&soe->list);
   148         list_del(&soe->list);
   138         ec_soe_request_clear(soe);
   149         ec_soe_request_clear(soe);
   139         kfree(soe);
   150         kfree(soe);
   140     }
   151     }
       
   152 
       
   153     ec_coe_emerg_ring_clear(&sc->emerg_ring);
   141 }
   154 }
   142 
   155 
   143 /*****************************************************************************/
   156 /*****************************************************************************/
   144 
   157 
   145 /** Prepares an FMMU configuration.
   158 /** Prepares an FMMU configuration.
   160         uint8_t sync_index, /**< Sync manager index. */
   173         uint8_t sync_index, /**< Sync manager index. */
   161         ec_direction_t dir /**< PDO direction. */
   174         ec_direction_t dir /**< PDO direction. */
   162         )
   175         )
   163 {
   176 {
   164     unsigned int i;
   177     unsigned int i;
   165     ec_fmmu_config_t *fmmu, *prev_fmmu;
   178     ec_fmmu_config_t *fmmu;
   166     uint32_t fmmu_logical_start_address;
       
   167     size_t tx_size, old_prev_tx_size;
       
   168 
   179 
   169     // FMMU configuration already prepared?
   180     // FMMU configuration already prepared?
   170     for (i = 0; i < sc->used_fmmus; i++) {
   181     for (i = 0; i < sc->used_fmmus; i++) {
   171         fmmu = &sc->fmmu_configs[i];
   182         fmmu = &sc->fmmu_configs[i];
   172         if (fmmu->domain == domain && fmmu->sync_index == sync_index)
   183         if (fmmu->domain == domain && fmmu->sync_index == sync_index)
   173             return fmmu->domain_address;
   184             return fmmu->logical_start_address;
   174     }
   185     }
   175 
   186 
   176     if (sc->used_fmmus == EC_MAX_FMMUS) {
   187     if (sc->used_fmmus == EC_MAX_FMMUS) {
   177         EC_CONFIG_ERR(sc, "FMMU limit reached!\n");
   188         EC_CONFIG_ERR(sc, "FMMU limit reached!\n");
   178         return -EOVERFLOW;
   189         return -EOVERFLOW;
   179     }
   190     }
   180 
   191 
   181     fmmu = &sc->fmmu_configs[sc->used_fmmus];
   192     fmmu = &sc->fmmu_configs[sc->used_fmmus++];
   182 
   193 
   183     ec_mutex_lock(&sc->master->master_mutex);
   194     down(&sc->master->master_sem);
   184     ec_fmmu_config_init(fmmu, sc, sync_index, dir);
   195     ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir);
   185     fmmu_logical_start_address = domain->tx_size;
   196     up(&sc->master->master_sem);
   186     tx_size = fmmu->data_size;
   197 
   187 
   198     return fmmu->logical_start_address;
   188     // FIXME is it enough to take only the *previous* FMMU into account?
       
   189 
       
   190     if (sc->allow_overlapping_pdos && sc->used_fmmus > 0) {
       
   191         prev_fmmu = &sc->fmmu_configs[sc->used_fmmus - 1];
       
   192         if (fmmu->dir != prev_fmmu->dir && prev_fmmu->tx_size != 0) {
       
   193             // prev fmmu has opposite direction
       
   194             // and is not already paired with prev-prev fmmu
       
   195             old_prev_tx_size = prev_fmmu->tx_size;
       
   196             prev_fmmu->tx_size = max(fmmu->data_size, prev_fmmu->data_size);
       
   197             domain->tx_size += prev_fmmu->tx_size - old_prev_tx_size;
       
   198             tx_size = 0;
       
   199             fmmu_logical_start_address = prev_fmmu->logical_start_address;
       
   200         }
       
   201     }
       
   202 
       
   203     ec_fmmu_config_domain(fmmu, domain, fmmu_logical_start_address, tx_size);
       
   204     ec_mutex_unlock(&sc->master->master_mutex);
       
   205 
       
   206     sc->used_fmmus++;
       
   207     return fmmu->domain_address;
       
   208 }
   199 }
   209 
   200 
   210 /*****************************************************************************/
   201 /*****************************************************************************/
   211 
   202 
   212 /** Attaches the configuration to the addressed slave object.
   203 /** Attaches the configuration to the addressed slave object.
   263     // attach slave
   254     // attach slave
   264     slave->config = sc;
   255     slave->config = sc;
   265     sc->slave = slave;
   256     sc->slave = slave;
   266 
   257 
   267     EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position);
   258     EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position);
   268 
       
   269     return 0;
   259     return 0;
   270 }
   260 }
   271 
   261 
   272 /*****************************************************************************/
   262 /*****************************************************************************/
   273 
   263 
   276 void ec_slave_config_detach(
   266 void ec_slave_config_detach(
   277         ec_slave_config_t *sc /**< Slave configuration. */
   267         ec_slave_config_t *sc /**< Slave configuration. */
   278         )
   268         )
   279 {
   269 {
   280     if (sc->slave) {
   270     if (sc->slave) {
       
   271         ec_reg_request_t *reg;
       
   272 
   281         sc->slave->config = NULL;
   273         sc->slave->config = NULL;
       
   274 
       
   275         // invalidate processing register request
       
   276         list_for_each_entry(reg, &sc->reg_requests, list) {
       
   277             if (sc->slave->fsm.reg_request == reg) {
       
   278                 sc->slave->fsm.reg_request = NULL;
       
   279                 break;
       
   280             }
       
   281         }
       
   282 
   282         sc->slave = NULL;
   283         sc->slave = NULL;
   283     }
   284     }
   284 }
   285 }
   285 
   286 
   286 /*****************************************************************************/
   287 /*****************************************************************************/
   293     ec_sync_config_t *sync_config;
   294     ec_sync_config_t *sync_config;
   294     const ec_sync_t *sync;
   295     const ec_sync_t *sync;
   295 
   296 
   296     if (!sc->slave)
   297     if (!sc->slave)
   297         return;
   298         return;
   298     
   299 
   299     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   300     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   300         sync_config = &sc->sync_configs[sync_index];
   301         sync_config = &sc->sync_configs[sync_index];
   301         if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {
   302         if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {
   302             sync_config->dir = ec_sync_default_direction(sync);
   303             sync_config->dir = ec_sync_default_direction(sync);
   303             if (sync_config->dir == EC_DIR_INVALID)
   304             if (sync_config->dir == EC_DIR_INVALID)
   385 /*****************************************************************************/
   386 /*****************************************************************************/
   386 
   387 
   387 /** Finds an SDO configuration via its position in the list.
   388 /** Finds an SDO configuration via its position in the list.
   388  *
   389  *
   389  * Const version.
   390  * Const version.
       
   391  *
       
   392  * \return Search result, or NULL.
   390  */
   393  */
   391 const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const(
   394 const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const(
   392         const ec_slave_config_t *sc, /**< Slave configuration. */
   395         const ec_slave_config_t *sc, /**< Slave configuration. */
   393         unsigned int pos /**< Position in the list. */
   396         unsigned int pos /**< Position in the list. */
   394         )
   397         )
   427 /*****************************************************************************/
   430 /*****************************************************************************/
   428 
   431 
   429 /** Finds an IDN configuration via its position in the list.
   432 /** Finds an IDN configuration via its position in the list.
   430  *
   433  *
   431  * Const version.
   434  * Const version.
       
   435  *
       
   436  * \return Search result, or NULL.
   432  */
   437  */
   433 const ec_soe_request_t *ec_slave_config_get_idn_by_pos_const(
   438 const ec_soe_request_t *ec_slave_config_get_idn_by_pos_const(
   434         const ec_slave_config_t *sc, /**< Slave configuration. */
   439         const ec_slave_config_t *sc, /**< Slave configuration. */
   435         unsigned int pos /**< Position in the list. */
   440         unsigned int pos /**< Position in the list. */
   436         )
   441         )
   446     return NULL;
   451     return NULL;
   447 }
   452 }
   448 
   453 
   449 /*****************************************************************************/
   454 /*****************************************************************************/
   450 
   455 
   451 /** Finds a VoE handler via its position in the list.
   456 /** Finds a CoE handler via its position in the list.
       
   457  *
       
   458  * \return Search result, or NULL.
   452  */
   459  */
   453 ec_sdo_request_t *ec_slave_config_find_sdo_request(
   460 ec_sdo_request_t *ec_slave_config_find_sdo_request(
   454         ec_slave_config_t *sc, /**< Slave configuration. */
   461         ec_slave_config_t *sc, /**< Slave configuration. */
   455         unsigned int pos /**< Position in the list. */
   462         unsigned int pos /**< Position in the list. */
   456         )
   463         )
   466     return NULL;
   473     return NULL;
   467 }
   474 }
   468 
   475 
   469 /*****************************************************************************/
   476 /*****************************************************************************/
   470 
   477 
       
   478 /** Finds a register handler via its position in the list.
       
   479  *
       
   480  * \return Search result, or NULL.
       
   481  */
       
   482 ec_reg_request_t *ec_slave_config_find_reg_request(
       
   483         ec_slave_config_t *sc, /**< Slave configuration. */
       
   484         unsigned int pos /**< Position in the list. */
       
   485         )
       
   486 {
       
   487     ec_reg_request_t *reg;
       
   488 
       
   489     list_for_each_entry(reg, &sc->reg_requests, list) {
       
   490         if (pos--)
       
   491             continue;
       
   492         return reg;
       
   493     }
       
   494 
       
   495     return NULL;
       
   496 }
       
   497 
       
   498 /*****************************************************************************/
       
   499 
   471 /** Finds a VoE handler via its position in the list.
   500 /** Finds a VoE handler via its position in the list.
       
   501  *
       
   502  * \return Search result, or NULL.
   472  */
   503  */
   473 ec_voe_handler_t *ec_slave_config_find_voe_handler(
   504 ec_voe_handler_t *ec_slave_config_find_voe_handler(
   474         ec_slave_config_t *sc, /**< Slave configuration. */
   505         ec_slave_config_t *sc, /**< Slave configuration. */
   475         unsigned int pos /**< Position in the list. */
   506         unsigned int pos /**< Position in the list. */
   476         )
   507         )
   492 
   523 
   493 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
   524 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
   494         ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
   525         ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
   495 {
   526 {
   496     ec_sync_config_t *sync_config;
   527     ec_sync_config_t *sync_config;
   497     
   528 
   498     EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p,"
   529     EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p,"
   499             " sync_index = %u, dir = %i, watchdog_mode = %i)\n",
   530             " sync_index = %u, dir = %i, watchdog_mode = %i)\n",
   500             sc, sync_index, dir, watchdog_mode);
   531             sc, sync_index, dir, watchdog_mode);
   501 
   532 
   502     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   533     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   527     sc->watchdog_intervals = intervals;
   558     sc->watchdog_intervals = intervals;
   528 }
   559 }
   529 
   560 
   530 /*****************************************************************************/
   561 /*****************************************************************************/
   531 
   562 
   532 void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc,
       
   533         uint8_t allow_overlapping_pdos )
       
   534 {
       
   535     if (sc->master->debug_level)
       
   536         EC_DBG("%s(sc = 0x%p, allow_overlapping_pdos = %u)\n",
       
   537                 __func__, sc, allow_overlapping_pdos);
       
   538 
       
   539     sc->allow_overlapping_pdos = allow_overlapping_pdos;
       
   540 }
       
   541 
       
   542 /*****************************************************************************/
       
   543 
       
   544 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
   563 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
   545         uint8_t sync_index, uint16_t pdo_index)
   564         uint8_t sync_index, uint16_t pdo_index)
   546 {
   565 {
   547     ec_pdo_t *pdo;
   566     ec_pdo_t *pdo;
   548 
   567 
   552     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   571     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   553         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   572         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   554         return -EINVAL;
   573         return -EINVAL;
   555     }
   574     }
   556 
   575 
   557     ec_mutex_lock(&sc->master->master_mutex);
   576     down(&sc->master->master_sem);
   558 
   577 
   559     pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index);
   578     pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index);
   560     if (IS_ERR(pdo)) {
   579     if (IS_ERR(pdo)) {
   561         ec_mutex_unlock(&sc->master->master_mutex);
   580         up(&sc->master->master_sem);
   562         return PTR_ERR(pdo);
   581         return PTR_ERR(pdo);
   563     }
   582     }
   564     pdo->sync_index = sync_index;
   583     pdo->sync_index = sync_index;
   565 
   584 
   566     ec_slave_config_load_default_mapping(sc, pdo);
   585     ec_slave_config_load_default_mapping(sc, pdo);
   567 
   586 
   568     ec_mutex_unlock(&sc->master->master_mutex);
   587     up(&sc->master->master_sem);
   569     return 0;
   588     return 0;
   570 }
   589 }
   571 
   590 
   572 /*****************************************************************************/
   591 /*****************************************************************************/
   573 
   592 
   580     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   599     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   581         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   600         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   582         return;
   601         return;
   583     }
   602     }
   584 
   603 
   585     ec_mutex_lock(&sc->master->master_mutex);
   604     down(&sc->master->master_sem);
   586     ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
   605     ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
   587     ec_mutex_unlock(&sc->master->master_mutex);
   606     up(&sc->master->master_sem);
   588 }
   607 }
   589 
   608 
   590 /*****************************************************************************/
   609 /*****************************************************************************/
   591 
   610 
   592 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
   611 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
   595 {
   614 {
   596     uint8_t sync_index;
   615     uint8_t sync_index;
   597     ec_pdo_t *pdo = NULL;
   616     ec_pdo_t *pdo = NULL;
   598     ec_pdo_entry_t *entry;
   617     ec_pdo_entry_t *entry;
   599     int retval = 0;
   618     int retval = 0;
   600     
   619 
   601     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
   620     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
   602             "pdo_index = 0x%04X, entry_index = 0x%04X, "
   621             "pdo_index = 0x%04X, entry_index = 0x%04X, "
   603             "entry_subindex = 0x%02X, entry_bit_length = %u)\n",
   622             "entry_subindex = 0x%02X, entry_bit_length = %u)\n",
   604             __func__, sc, pdo_index, entry_index, entry_subindex,
   623             __func__, sc, pdo_index, entry_index, entry_subindex,
   605             entry_bit_length);
   624             entry_bit_length);
   608         if ((pdo = ec_pdo_list_find_pdo(
   627         if ((pdo = ec_pdo_list_find_pdo(
   609                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   628                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   610             break;
   629             break;
   611 
   630 
   612     if (pdo) {
   631     if (pdo) {
   613         ec_mutex_lock(&sc->master->master_mutex);
   632         down(&sc->master->master_sem);
   614         entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex,
   633         entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex,
   615                 entry_bit_length);
   634                 entry_bit_length);
   616         ec_mutex_unlock(&sc->master->master_mutex);
   635         up(&sc->master->master_sem);
   617         if (IS_ERR(entry))
   636         if (IS_ERR(entry))
   618             retval = PTR_ERR(entry);
   637             retval = PTR_ERR(entry);
   619     } else {
   638     } else {
   620         EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
   639         EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
   621         retval = -ENOENT; 
   640         retval = -ENOENT;
   622     }
   641     }
   623 
   642 
   624     return retval;
   643     return retval;
   625 }
   644 }
   626 
   645 
   629 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
   648 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
   630         uint16_t pdo_index)
   649         uint16_t pdo_index)
   631 {
   650 {
   632     uint8_t sync_index;
   651     uint8_t sync_index;
   633     ec_pdo_t *pdo = NULL;
   652     ec_pdo_t *pdo = NULL;
   634     
   653 
   635     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n",
   654     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n",
   636             __func__, sc, pdo_index);
   655             __func__, sc, pdo_index);
   637 
   656 
   638     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
   657     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
   639         if ((pdo = ec_pdo_list_find_pdo(
   658         if ((pdo = ec_pdo_list_find_pdo(
   640                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   659                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   641             break;
   660             break;
   642 
   661 
   643     if (pdo) {
   662     if (pdo) {
   644         ec_mutex_lock(&sc->master->master_mutex);
   663         down(&sc->master->master_sem);
   645         ec_pdo_clear_entries(pdo);
   664         ec_pdo_clear_entries(pdo);
   646         ec_mutex_unlock(&sc->master->master_mutex);
   665         up(&sc->master->master_sem);
   647     } else {
   666     } else {
   648         EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
   667         EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
   649     }
   668     }
   650 }
   669 }
   651 
   670 
   729     const ec_sync_config_t *sync_config;
   748     const ec_sync_config_t *sync_config;
   730     unsigned int bit_offset, bit_pos;
   749     unsigned int bit_offset, bit_pos;
   731     ec_pdo_t *pdo;
   750     ec_pdo_t *pdo;
   732     ec_pdo_entry_t *entry;
   751     ec_pdo_entry_t *entry;
   733     int sync_offset;
   752     int sync_offset;
       
   753 
       
   754     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
       
   755             "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n",
       
   756             __func__, sc, index, subindex, domain, bit_position);
   734 
   757 
   735     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   758     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   736         sync_config = &sc->sync_configs[sync_index];
   759         sync_config = &sc->sync_configs[sync_index];
   737         bit_offset = 0;
   760         bit_offset = 0;
   738 
   761 
   753                     sync_offset = ec_slave_config_prepare_fmmu(
   776                     sync_offset = ec_slave_config_prepare_fmmu(
   754                             sc, domain, sync_index, sync_config->dir);
   777                             sc, domain, sync_index, sync_config->dir);
   755                     if (sync_offset < 0)
   778                     if (sync_offset < 0)
   756                         return sync_offset;
   779                         return sync_offset;
   757 
   780 
   758                     EC_CONFIG_DBG(sc, 1, "%s(index = 0x%04X, "
   781                     return sync_offset + bit_offset / 8;
   759                                   "subindex = 0x%02X, domain = %u, bytepos=%u, bitpos=%u)\n",
       
   760                                   __func__,index, subindex,
       
   761                                   domain->index, sync_offset + bit_offset / 8, bit_pos);
       
   762 					return sync_offset + bit_offset / 8;
       
   763                 }
   782                 }
   764             }
   783             }
   765         }
   784         }
   766     }
   785     }
   767 
   786 
   770     return -ENOENT;
   789     return -ENOENT;
   771 }
   790 }
   772 
   791 
   773 /*****************************************************************************/
   792 /*****************************************************************************/
   774 
   793 
       
   794 int ecrt_slave_config_reg_pdo_entry_pos(
       
   795         ec_slave_config_t *sc,
       
   796         uint8_t sync_index,
       
   797         unsigned int pdo_pos,
       
   798         unsigned int entry_pos,
       
   799         ec_domain_t *domain,
       
   800         unsigned int *bit_position
       
   801         )
       
   802 {
       
   803     const ec_sync_config_t *sync_config;
       
   804     unsigned int bit_offset, pp, ep;
       
   805     ec_pdo_t *pdo;
       
   806     ec_pdo_entry_t *entry;
       
   807 
       
   808     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, pdo_pos = %u,"
       
   809             " entry_pos = %u, domain = 0x%p, bit_position = 0x%p)\n",
       
   810             __func__, sc, sync_index, pdo_pos, entry_pos,
       
   811             domain, bit_position);
       
   812 
       
   813     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
       
   814         EC_CONFIG_ERR(sc, "Invalid syncmanager position %u.\n", sync_index);
       
   815         return -EINVAL;
       
   816     }
       
   817 
       
   818     sync_config = &sc->sync_configs[sync_index];
       
   819     bit_offset = 0;
       
   820     pp = 0;
       
   821 
       
   822     list_for_each_entry(pdo, &sync_config->pdos.list, list) {
       
   823         ep = 0;
       
   824         list_for_each_entry(entry, &pdo->entries, list) {
       
   825             if (pp != pdo_pos || ep != entry_pos) {
       
   826                 bit_offset += entry->bit_length;
       
   827             } else {
       
   828                 unsigned int bit_pos = bit_offset % 8;
       
   829                 int sync_offset;
       
   830 
       
   831                 if (bit_position) {
       
   832                     *bit_position = bit_pos;
       
   833                 } else if (bit_pos) {
       
   834                     EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does"
       
   835                             " not byte-align.\n",
       
   836                             pdo->index, entry->subindex);
       
   837                     return -EFAULT;
       
   838                 }
       
   839 
       
   840                 sync_offset = ec_slave_config_prepare_fmmu(
       
   841                         sc, domain, sync_index, sync_config->dir);
       
   842                 if (sync_offset < 0)
       
   843                     return sync_offset;
       
   844 
       
   845                 return sync_offset + bit_offset / 8;
       
   846             }
       
   847             ep++;
       
   848         }
       
   849         pp++;
       
   850     }
       
   851 
       
   852     EC_CONFIG_ERR(sc, "PDO entry specification %u/%u/%u out of range.\n",
       
   853            sync_index, pdo_pos, entry_pos);
       
   854     return -ENOENT;
       
   855 }
       
   856 
       
   857 /*****************************************************************************/
       
   858 
   775 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,
   859 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,
   776         uint32_t sync0_cycle_time, uint32_t sync0_shift_time,
   860         uint32_t sync0_cycle_time, int32_t sync0_shift_time,
   777         uint32_t sync1_cycle_time, uint32_t sync1_shift_time)
   861         uint32_t sync1_cycle_time, int32_t sync1_shift_time)
   778 {
   862 {
   779     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, assign_activate = 0x%04X,"
   863     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, assign_activate = 0x%04X,"
   780             " sync0_cycle = %u, sync0_shift = %u,"
   864             " sync0_cycle = %u, sync0_shift = %i,"
   781             " sync1_cycle = %u, sync1_shift = %u\n",
   865             " sync1_cycle = %u, sync1_shift = %i\n",
   782             __func__, sc, assign_activate, sync0_cycle_time, sync0_shift_time,
   866             __func__, sc, assign_activate, sync0_cycle_time, sync0_shift_time,
   783             sync1_cycle_time, sync1_shift_time);
   867             sync1_cycle_time, sync1_shift_time);
   784 
   868 
   785     sc->dc_assign_activate = assign_activate;
   869     sc->dc_assign_activate = assign_activate;
   786     sc->dc_sync[0].cycle_time = sync0_cycle_time;
   870     sc->dc_sync[0].cycle_time = sync0_cycle_time;
   812                 " SDO configuration!\n");
   896                 " SDO configuration!\n");
   813         return -ENOMEM;
   897         return -ENOMEM;
   814     }
   898     }
   815 
   899 
   816     ec_sdo_request_init(req);
   900     ec_sdo_request_init(req);
   817     ec_sdo_request_address(req, index, subindex);
   901     ecrt_sdo_request_index(req, index, subindex);
   818 
   902 
   819     ret = ec_sdo_request_copy_data(req, data, size);
   903     ret = ec_sdo_request_copy_data(req, data, size);
   820     if (ret < 0) {
   904     if (ret < 0) {
   821         ec_sdo_request_clear(req);
   905         ec_sdo_request_clear(req);
   822         kfree(req);
   906         kfree(req);
   823         return ret;
   907         return ret;
   824     }
   908     }
   825         
   909 
   826     ec_mutex_lock(&sc->master->master_mutex);
   910     down(&sc->master->master_sem);
   827     list_add_tail(&req->list, &sc->sdo_configs);
   911     list_add_tail(&req->list, &sc->sdo_configs);
   828     ec_mutex_unlock(&sc->master->master_mutex);
   912     up(&sc->master->master_sem);
   829     return 0;
   913     return 0;
   830 }
   914 }
   831 
   915 
   832 /*****************************************************************************/
   916 /*****************************************************************************/
   833 
   917 
   896                 " SDO configuration!\n");
   980                 " SDO configuration!\n");
   897         return -ENOMEM;
   981         return -ENOMEM;
   898     }
   982     }
   899 
   983 
   900     ec_sdo_request_init(req);
   984     ec_sdo_request_init(req);
   901     ec_sdo_request_address(req, index, 0);
   985     ecrt_sdo_request_index(req, index, 0);
   902     req->complete_access = 1;
   986     req->complete_access = 1;
   903 
   987 
   904     ret = ec_sdo_request_copy_data(req, data, size);
   988     ret = ec_sdo_request_copy_data(req, data, size);
   905     if (ret < 0) {
   989     if (ret < 0) {
   906         ec_sdo_request_clear(req);
   990         ec_sdo_request_clear(req);
   907         kfree(req);
   991         kfree(req);
   908         return ret;
   992         return ret;
   909     }
   993     }
   910         
   994 
   911     ec_mutex_lock(&sc->master->master_mutex);
   995     down(&sc->master->master_sem);
   912     list_add_tail(&req->list, &sc->sdo_configs);
   996     list_add_tail(&req->list, &sc->sdo_configs);
   913     ec_mutex_unlock(&sc->master->master_mutex);
   997     up(&sc->master->master_sem);
   914     return 0;
   998     return 0;
       
   999 }
       
  1000 
       
  1001 /*****************************************************************************/
       
  1002 
       
  1003 int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)
       
  1004 {
       
  1005     return ec_coe_emerg_ring_size(&sc->emerg_ring, elements);
       
  1006 }
       
  1007 
       
  1008 /*****************************************************************************/
       
  1009 
       
  1010 int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)
       
  1011 {
       
  1012     return ec_coe_emerg_ring_pop(&sc->emerg_ring, target);
       
  1013 }
       
  1014 
       
  1015 /*****************************************************************************/
       
  1016 
       
  1017 int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc)
       
  1018 {
       
  1019     return ec_coe_emerg_ring_clear_ring(&sc->emerg_ring);
       
  1020 }
       
  1021 
       
  1022 /*****************************************************************************/
       
  1023 
       
  1024 int ecrt_slave_config_emerg_overruns(ec_slave_config_t *sc)
       
  1025 {
       
  1026     return ec_coe_emerg_ring_overruns(&sc->emerg_ring);
   915 }
  1027 }
   916 
  1028 
   917 /*****************************************************************************/
  1029 /*****************************************************************************/
   918 
  1030 
   919 /** Same as ecrt_slave_config_create_sdo_request(), but with ERR_PTR() return
  1031 /** Same as ecrt_slave_config_create_sdo_request(), but with ERR_PTR() return
   934         EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n");
  1046         EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n");
   935         return ERR_PTR(-ENOMEM);
  1047         return ERR_PTR(-ENOMEM);
   936     }
  1048     }
   937 
  1049 
   938     ec_sdo_request_init(req);
  1050     ec_sdo_request_init(req);
   939     ec_sdo_request_address(req, index, subindex);
  1051     ecrt_sdo_request_index(req, index, subindex);
   940 
  1052 
   941     ret = ec_sdo_request_alloc(req, size);
  1053     ret = ec_sdo_request_alloc(req, size);
   942     if (ret < 0) {
  1054     if (ret < 0) {
   943         ec_sdo_request_clear(req);
  1055         ec_sdo_request_clear(req);
   944         kfree(req);
  1056         kfree(req);
   946     }
  1058     }
   947 
  1059 
   948     // prepare data for optional writing
  1060     // prepare data for optional writing
   949     memset(req->data, 0x00, size);
  1061     memset(req->data, 0x00, size);
   950     req->data_size = size;
  1062     req->data_size = size;
   951     
  1063 
   952     ec_mutex_lock(&sc->master->master_mutex);
  1064     down(&sc->master->master_sem);
   953     list_add_tail(&req->list, &sc->sdo_requests);
  1065     list_add_tail(&req->list, &sc->sdo_requests);
   954     ec_mutex_unlock(&sc->master->master_mutex);
  1066     up(&sc->master->master_sem);
   955 
  1067 
   956     return req; 
  1068     return req;
   957 }
  1069 }
   958 
  1070 
   959 /*****************************************************************************/
  1071 /*****************************************************************************/
   960 
  1072 
   961 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
  1073 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
   962         ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
  1074         ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
   963 {
  1075 {
   964     ec_sdo_request_t *s = ecrt_slave_config_create_sdo_request_err(sc, index,
  1076     ec_sdo_request_t *s = ecrt_slave_config_create_sdo_request_err(sc, index,
   965             subindex, size);
  1077             subindex, size);
   966     return IS_ERR(s) ? NULL : s;
  1078     return IS_ERR(s) ? NULL : s;
       
  1079 }
       
  1080 
       
  1081 /*****************************************************************************/
       
  1082 
       
  1083 /** Same as ecrt_slave_config_create_reg_request(), but with ERR_PTR() return
       
  1084  * value.
       
  1085  */
       
  1086 ec_reg_request_t *ecrt_slave_config_create_reg_request_err(
       
  1087         ec_slave_config_t *sc, size_t size)
       
  1088 {
       
  1089     ec_reg_request_t *reg;
       
  1090     int ret;
       
  1091 
       
  1092     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n",
       
  1093             __func__, sc, size);
       
  1094 
       
  1095     if (!(reg = (ec_reg_request_t *)
       
  1096                 kmalloc(sizeof(ec_reg_request_t), GFP_KERNEL))) {
       
  1097         EC_CONFIG_ERR(sc, "Failed to allocate register request memory!\n");
       
  1098         return ERR_PTR(-ENOMEM);
       
  1099     }
       
  1100 
       
  1101     ret = ec_reg_request_init(reg, size);
       
  1102     if (ret) {
       
  1103         kfree(reg);
       
  1104         return ERR_PTR(ret);
       
  1105     }
       
  1106 
       
  1107     down(&sc->master->master_sem);
       
  1108     list_add_tail(&reg->list, &sc->reg_requests);
       
  1109     up(&sc->master->master_sem);
       
  1110 
       
  1111     return reg;
       
  1112 }
       
  1113 
       
  1114 /*****************************************************************************/
       
  1115 
       
  1116 ec_reg_request_t *ecrt_slave_config_create_reg_request(
       
  1117         ec_slave_config_t *sc, size_t size)
       
  1118 {
       
  1119     ec_reg_request_t *reg =
       
  1120         ecrt_slave_config_create_reg_request_err(sc, size);
       
  1121     return IS_ERR(reg) ? NULL : reg;
   967 }
  1122 }
   968 
  1123 
   969 /*****************************************************************************/
  1124 /*****************************************************************************/
   970 
  1125 
   971 /** Same as ecrt_slave_config_create_voe_handler(), but with ERR_PTR() return
  1126 /** Same as ecrt_slave_config_create_voe_handler(), but with ERR_PTR() return
   989     if (ret < 0) {
  1144     if (ret < 0) {
   990         kfree(voe);
  1145         kfree(voe);
   991         return ERR_PTR(ret);
  1146         return ERR_PTR(ret);
   992     }
  1147     }
   993 
  1148 
   994     ec_mutex_lock(&sc->master->master_mutex);
  1149     down(&sc->master->master_sem);
   995     list_add_tail(&voe->list, &sc->voe_handlers);
  1150     list_add_tail(&voe->list, &sc->voe_handlers);
   996     ec_mutex_unlock(&sc->master->master_mutex);
  1151     up(&sc->master->master_sem);
   997 
  1152 
   998     return voe; 
  1153     return voe;
   999 }
  1154 }
  1000 
  1155 
  1001 /*****************************************************************************/
  1156 /*****************************************************************************/
  1002 
  1157 
  1003 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(
  1158 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(
  1025     }
  1180     }
  1026 }
  1181 }
  1027 
  1182 
  1028 /*****************************************************************************/
  1183 /*****************************************************************************/
  1029 
  1184 
  1030 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, 
  1185 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
  1031         uint16_t idn, ec_al_state_t state, const uint8_t *data,
  1186         uint16_t idn, ec_al_state_t state, const uint8_t *data,
  1032         size_t size)
  1187         size_t size)
  1033 {
  1188 {
  1034     ec_slave_t *slave = sc->slave;
  1189     ec_slave_t *slave = sc->slave;
  1035     ec_soe_request_t *req;
  1190     ec_soe_request_t *req;
  1071     if (ret < 0) {
  1226     if (ret < 0) {
  1072         ec_soe_request_clear(req);
  1227         ec_soe_request_clear(req);
  1073         kfree(req);
  1228         kfree(req);
  1074         return ret;
  1229         return ret;
  1075     }
  1230     }
  1076         
  1231 
  1077     ec_mutex_lock(&sc->master->master_mutex);
  1232     down(&sc->master->master_sem);
  1078     list_add_tail(&req->list, &sc->soe_configs);
  1233     list_add_tail(&req->list, &sc->soe_configs);
  1079     ec_mutex_unlock(&sc->master->master_mutex);
  1234     up(&sc->master->master_sem);
  1080     return 0;
  1235     return 0;
  1081 }
  1236 }
  1082 
  1237 
  1083 /*****************************************************************************/
  1238 /*****************************************************************************/
  1084 
  1239 
  1085 /** \cond */
  1240 /** \cond */
  1086 
  1241 
  1087 EXPORT_SYMBOL(ecrt_slave_config_sync_manager);
  1242 EXPORT_SYMBOL(ecrt_slave_config_sync_manager);
  1088 EXPORT_SYMBOL(ecrt_slave_config_watchdog);
  1243 EXPORT_SYMBOL(ecrt_slave_config_watchdog);
  1089 EXPORT_SYMBOL(ecrt_slave_config_overlapping_pdos);
       
  1090 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add);
  1244 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add);
  1091 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear);
  1245 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear);
  1092 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add);
  1246 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add);
  1093 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear);
  1247 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear);
  1094 EXPORT_SYMBOL(ecrt_slave_config_pdos);
  1248 EXPORT_SYMBOL(ecrt_slave_config_pdos);
  1097 EXPORT_SYMBOL(ecrt_slave_config_sdo);
  1251 EXPORT_SYMBOL(ecrt_slave_config_sdo);
  1098 EXPORT_SYMBOL(ecrt_slave_config_sdo8);
  1252 EXPORT_SYMBOL(ecrt_slave_config_sdo8);
  1099 EXPORT_SYMBOL(ecrt_slave_config_sdo16);
  1253 EXPORT_SYMBOL(ecrt_slave_config_sdo16);
  1100 EXPORT_SYMBOL(ecrt_slave_config_sdo32);
  1254 EXPORT_SYMBOL(ecrt_slave_config_sdo32);
  1101 EXPORT_SYMBOL(ecrt_slave_config_complete_sdo);
  1255 EXPORT_SYMBOL(ecrt_slave_config_complete_sdo);
       
  1256 EXPORT_SYMBOL(ecrt_slave_config_emerg_size);
       
  1257 EXPORT_SYMBOL(ecrt_slave_config_emerg_pop);
       
  1258 EXPORT_SYMBOL(ecrt_slave_config_emerg_clear);
       
  1259 EXPORT_SYMBOL(ecrt_slave_config_emerg_overruns);
  1102 EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request);
  1260 EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request);
  1103 EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler);
  1261 EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler);
       
  1262 EXPORT_SYMBOL(ecrt_slave_config_create_reg_request);
  1104 EXPORT_SYMBOL(ecrt_slave_config_state);
  1263 EXPORT_SYMBOL(ecrt_slave_config_state);
  1105 EXPORT_SYMBOL(ecrt_slave_config_idn);
  1264 EXPORT_SYMBOL(ecrt_slave_config_idn);
  1106 
  1265 
  1107 /** \endcond */
  1266 /** \endcond */
  1108 
  1267