master/slave.c
changeset 1327 4d179b06dd3c
parent 1326 ef907b0b5125
child 1337 0253c74d0940
equal deleted inserted replaced
1326:ef907b0b5125 1327:4d179b06dd3c
   138     ec_pdo_t *pdo, *next_pdo;
   138     ec_pdo_t *pdo, *next_pdo;
   139 
   139 
   140     if (slave->config)
   140     if (slave->config)
   141         ec_slave_config_detach(slave->config);
   141         ec_slave_config_detach(slave->config);
   142 
   142 
   143     // free all Sdos
   143     // free all SDOs
   144     list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
   144     list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
   145         list_del(&sdo->list);
   145         list_del(&sdo->list);
   146         ec_sdo_clear(sdo);
   146         ec_sdo_clear(sdo);
   147         kfree(sdo);
   147         kfree(sdo);
   148     }
   148     }
   155     }
   155     }
   156 
   156 
   157     // free all sync managers
   157     // free all sync managers
   158     ec_slave_clear_sync_managers(slave);
   158     ec_slave_clear_sync_managers(slave);
   159 
   159 
   160     // free all SII Pdos
   160     // free all SII PDOs
   161     list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) {
   161     list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) {
   162         list_del(&pdo->list);
   162         list_del(&pdo->list);
   163         ec_pdo_clear(pdo);
   163         ec_pdo_clear(pdo);
   164         kfree(pdo);
   164         kfree(pdo);
   165     }
   165     }
   396 }
   396 }
   397 
   397 
   398 /*****************************************************************************/
   398 /*****************************************************************************/
   399 
   399 
   400 /**
   400 /**
   401    Fetches data from a [RT]XPdo category.
   401    Fetches data from a [RT]xPDO category.
   402    \return 0 in case of success, else < 0
   402    \return 0 in case of success, else < 0
   403 */
   403 */
   404 
   404 
   405 int ec_slave_fetch_sii_pdos(
   405 int ec_slave_fetch_sii_pdos(
   406         ec_slave_t *slave, /**< EtherCAT slave */
   406         ec_slave_t *slave, /**< EtherCAT slave */
   407         const uint8_t *data, /**< category data */
   407         const uint8_t *data, /**< category data */
   408         size_t data_size, /**< number of bytes */
   408         size_t data_size, /**< number of bytes */
   409         ec_direction_t dir /**< Pdo direction. */
   409         ec_direction_t dir /**< PDO direction. */
   410         )
   410         )
   411 {
   411 {
   412     int ret;
   412     int ret;
   413     ec_pdo_t *pdo;
   413     ec_pdo_t *pdo;
   414     ec_pdo_entry_t *entry;
   414     ec_pdo_entry_t *entry;
   415     unsigned int entry_count, i;
   415     unsigned int entry_count, i;
   416 
   416 
   417     while (data_size >= 8) {
   417     while (data_size >= 8) {
   418         if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
   418         if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
   419             EC_ERR("Failed to allocate Pdo memory.\n");
   419             EC_ERR("Failed to allocate PDO memory.\n");
   420             return -ENOMEM;
   420             return -ENOMEM;
   421         }
   421         }
   422 
   422 
   423         ec_pdo_init(pdo);
   423         ec_pdo_init(pdo);
   424         pdo->index = EC_READ_U16(data);
   424         pdo->index = EC_READ_U16(data);
   436         data_size -= 8;
   436         data_size -= 8;
   437         data += 8;
   437         data += 8;
   438 
   438 
   439         for (i = 0; i < entry_count; i++) {
   439         for (i = 0; i < entry_count; i++) {
   440             if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
   440             if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
   441                 EC_ERR("Failed to allocate Pdo entry memory.\n");
   441                 EC_ERR("Failed to allocate PDO entry memory.\n");
   442                 return -ENOMEM;
   442                 return -ENOMEM;
   443             }
   443             }
   444 
   444 
   445             ec_pdo_entry_init(entry);
   445             ec_pdo_entry_init(entry);
   446             entry->index = EC_READ_U16(data);
   446             entry->index = EC_READ_U16(data);
   457 
   457 
   458             data_size -= 8;
   458             data_size -= 8;
   459             data += 8;
   459             data += 8;
   460         }
   460         }
   461 
   461 
   462         // if sync manager index is positive, the Pdo is mapped by default
   462         // if sync manager index is positive, the PDO is mapped by default
   463         if (pdo->sync_index >= 0) {
   463         if (pdo->sync_index >= 0) {
   464             ec_sync_t *sync;
   464             ec_sync_t *sync;
   465 
   465 
   466             if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) {
   466             if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) {
   467                 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.",
   467                 EC_ERR("Invalid SM index %i for PDO 0x%04X in slave %u.",
   468                         pdo->sync_index, pdo->index, slave->ring_position);
   468                         pdo->sync_index, pdo->index, slave->ring_position);
   469                 return -ENOENT;
   469                 return -ENOENT;
   470             }
   470             }
   471 
   471 
   472             ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo);
   472             ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo);
   522 }
   522 }
   523 
   523 
   524 /*****************************************************************************/
   524 /*****************************************************************************/
   525 
   525 
   526 /**
   526 /**
   527    Counts the total number of Sdos and entries in the dictionary.
   527    Counts the total number of SDOs and entries in the dictionary.
   528 */
   528 */
   529 
   529 
   530 void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */
   530 void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */
   531                             unsigned int *sdo_count, /**< number of Sdos */
   531                             unsigned int *sdo_count, /**< number of SDOs */
   532                             unsigned int *entry_count /**< total number of
   532                             unsigned int *entry_count /**< total number of
   533                                                          entries */
   533                                                          entries */
   534                             )
   534                             )
   535 {
   535 {
   536     unsigned int sdos = 0, entries = 0;
   536     unsigned int sdos = 0, entries = 0;
   549 }
   549 }
   550 
   550 
   551 /*****************************************************************************/
   551 /*****************************************************************************/
   552 
   552 
   553 /**
   553 /**
   554  * Get an Sdo from the dictionary.
   554  * Get an SDO from the dictionary.
   555  * \returns The desired Sdo, or NULL.
   555  * \returns The desired SDO, or NULL.
   556  */
   556  */
   557 
   557 
   558 ec_sdo_t *ec_slave_get_sdo(
   558 ec_sdo_t *ec_slave_get_sdo(
   559         ec_slave_t *slave, /**< EtherCAT slave */
   559         ec_slave_t *slave, /**< EtherCAT slave */
   560         uint16_t index /**< Sdo index */
   560         uint16_t index /**< SDO index */
   561         )
   561         )
   562 {
   562 {
   563     ec_sdo_t *sdo;
   563     ec_sdo_t *sdo;
   564 
   564 
   565     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
   565     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
   572 }
   572 }
   573 
   573 
   574 /*****************************************************************************/
   574 /*****************************************************************************/
   575 
   575 
   576 /**
   576 /**
   577  * Get an Sdo from the dictionary.
   577  * Get an SDO from the dictionary.
   578  *
   578  *
   579  * const version.
   579  * const version.
   580  *
   580  *
   581  * \returns The desired Sdo, or NULL.
   581  * \returns The desired SDO, or NULL.
   582  */
   582  */
   583 
   583 
   584 const ec_sdo_t *ec_slave_get_sdo_const(
   584 const ec_sdo_t *ec_slave_get_sdo_const(
   585         const ec_slave_t *slave, /**< EtherCAT slave */
   585         const ec_slave_t *slave, /**< EtherCAT slave */
   586         uint16_t index /**< Sdo index */
   586         uint16_t index /**< SDO index */
   587         )
   587         )
   588 {
   588 {
   589     const ec_sdo_t *sdo;
   589     const ec_sdo_t *sdo;
   590 
   590 
   591     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
   591     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
   597     return NULL;
   597     return NULL;
   598 }
   598 }
   599 
   599 
   600 /*****************************************************************************/
   600 /*****************************************************************************/
   601 
   601 
   602 /** Get an Sdo from the dictionary, given its position in the list.
   602 /** Get an SDO from the dictionary, given its position in the list.
   603  * \returns The desired Sdo, or NULL.
   603  * \returns The desired SDO, or NULL.
   604  */
   604  */
   605 
   605 
   606 const ec_sdo_t *ec_slave_get_sdo_by_pos_const(
   606 const ec_sdo_t *ec_slave_get_sdo_by_pos_const(
   607         const ec_slave_t *slave, /**< EtherCAT slave. */
   607         const ec_slave_t *slave, /**< EtherCAT slave. */
   608         uint16_t sdo_position /**< Sdo list position. */
   608         uint16_t sdo_position /**< SDO list position. */
   609         )
   609         )
   610 {
   610 {
   611     const ec_sdo_t *sdo;
   611     const ec_sdo_t *sdo;
   612 
   612 
   613     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
   613     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
   619     return NULL;
   619     return NULL;
   620 }
   620 }
   621 
   621 
   622 /*****************************************************************************/
   622 /*****************************************************************************/
   623 
   623 
   624 /** Get the number of Sdos in the dictionary.
   624 /** Get the number of SDOs in the dictionary.
   625  * \returns Sdo count.
   625  * \returns SDO count.
   626  */
   626  */
   627 
   627 
   628 uint16_t ec_slave_sdo_count(
   628 uint16_t ec_slave_sdo_count(
   629         const ec_slave_t *slave /**< EtherCAT slave. */
   629         const ec_slave_t *slave /**< EtherCAT slave. */
   630         )
   630         )
   639     return count;
   639     return count;
   640 }
   640 }
   641 
   641 
   642 /*****************************************************************************/
   642 /*****************************************************************************/
   643 
   643 
   644 /** Finds a mapped Pdo.
   644 /** Finds a mapped PDO.
   645  * \returns The desired Pdo object, or NULL.
   645  * \returns The desired PDO object, or NULL.
   646  */
   646  */
   647 const ec_pdo_t *ec_slave_find_pdo(
   647 const ec_pdo_t *ec_slave_find_pdo(
   648         const ec_slave_t *slave, /**< Slave. */
   648         const ec_slave_t *slave, /**< Slave. */
   649         uint16_t index /**< Pdo index to find. */
   649         uint16_t index /**< PDO index to find. */
   650         )
   650         )
   651 {
   651 {
   652     unsigned int i;
   652     unsigned int i;
   653     const ec_sync_t *sync;
   653     const ec_sync_t *sync;
   654     const ec_pdo_t *pdo;
   654     const ec_pdo_t *pdo;
   665     return NULL;
   665     return NULL;
   666 }
   666 }
   667 
   667 
   668 /*****************************************************************************/
   668 /*****************************************************************************/
   669 
   669 
   670 /** Find name for a Pdo and its entries.
   670 /** Find name for a PDO and its entries.
   671  */
   671  */
   672 void ec_slave_find_names_for_pdo(
   672 void ec_slave_find_names_for_pdo(
   673         ec_slave_t *slave,
   673         ec_slave_t *slave,
   674         ec_pdo_t *pdo
   674         ec_pdo_t *pdo
   675         )
   675         )
   696     }
   696     }
   697 }
   697 }
   698 
   698 
   699 /*****************************************************************************/
   699 /*****************************************************************************/
   700 
   700 
   701 /** Attach Pdo names.
   701 /** Attach PDO names.
   702  */
   702  */
   703 void ec_slave_attach_pdo_names(
   703 void ec_slave_attach_pdo_names(
   704         ec_slave_t *slave
   704         ec_slave_t *slave
   705         )
   705         )
   706 {
   706 {