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; |