176 if (kobject_add(&slave->kobj)) { |
176 if (kobject_add(&slave->kobj)) { |
177 EC_ERR("Failed to add slave's kobject.\n"); |
177 EC_ERR("Failed to add slave's kobject.\n"); |
178 goto out_slave_put; |
178 goto out_slave_put; |
179 } |
179 } |
180 |
180 |
181 // init SDO kobject and add it to the hierarchy |
181 // init Sdo kobject and add it to the hierarchy |
182 memset(&slave->sdo_kobj, 0x00, sizeof(struct kobject)); |
182 memset(&slave->sdo_kobj, 0x00, sizeof(struct kobject)); |
183 kobject_init(&slave->sdo_kobj); |
183 kobject_init(&slave->sdo_kobj); |
184 slave->sdo_kobj.ktype = &ktype_ec_slave_sdos; |
184 slave->sdo_kobj.ktype = &ktype_ec_slave_sdos; |
185 slave->sdo_kobj.parent = &slave->kobj; |
185 slave->sdo_kobj.parent = &slave->kobj; |
186 if (kobject_set_name(&slave->sdo_kobj, "sdos")) { |
186 if (kobject_set_name(&slave->sdo_kobj, "sdos")) { |
187 EC_ERR("Failed to set kobject name.\n"); |
187 EC_ERR("Failed to set kobject name.\n"); |
188 goto out_sdo_put; |
188 goto out_sdo_put; |
189 } |
189 } |
190 if (kobject_add(&slave->sdo_kobj)) { |
190 if (kobject_add(&slave->sdo_kobj)) { |
191 EC_ERR("Failed to add SDOs kobject.\n"); |
191 EC_ERR("Failed to add Sdos kobject.\n"); |
192 goto out_sdo_put; |
192 goto out_sdo_put; |
193 } |
193 } |
194 |
194 |
195 return 0; |
195 return 0; |
196 |
196 |
214 ec_sdo_t *sdo, *next_sdo; |
214 ec_sdo_t *sdo, *next_sdo; |
215 |
215 |
216 if (slave->config) |
216 if (slave->config) |
217 ec_slave_config_detach(slave->config); |
217 ec_slave_config_detach(slave->config); |
218 |
218 |
219 // free all SDOs |
219 // free all Sdos |
220 list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) { |
220 list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) { |
221 list_del(&sdo->list); |
221 list_del(&sdo->list); |
222 ec_sdo_destroy(sdo); |
222 ec_sdo_destroy(sdo); |
223 } |
223 } |
224 |
224 |
225 // free SDO kobject |
225 // free Sdo kobject |
226 kobject_del(&slave->sdo_kobj); |
226 kobject_del(&slave->sdo_kobj); |
227 kobject_put(&slave->sdo_kobj); |
227 kobject_put(&slave->sdo_kobj); |
228 |
228 |
229 // destroy self |
229 // destroy self |
230 kobject_del(&slave->kobj); |
230 kobject_del(&slave->kobj); |
260 ec_sync_clear(&slave->sii_syncs[i]); |
260 ec_sync_clear(&slave->sii_syncs[i]); |
261 } |
261 } |
262 kfree(slave->sii_syncs); |
262 kfree(slave->sii_syncs); |
263 } |
263 } |
264 |
264 |
265 // free all SII PDOs |
265 // free all SII Pdos |
266 list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { |
266 list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { |
267 list_del(&pdo->list); |
267 list_del(&pdo->list); |
268 ec_pdo_clear(pdo); |
268 ec_pdo_clear(pdo); |
269 kfree(pdo); |
269 kfree(pdo); |
270 } |
270 } |
488 } |
488 } |
489 |
489 |
490 /*****************************************************************************/ |
490 /*****************************************************************************/ |
491 |
491 |
492 /** |
492 /** |
493 Fetches data from a [RT]XPDO category. |
493 Fetches data from a [RT]XPdo category. |
494 \return 0 in case of success, else < 0 |
494 \return 0 in case of success, else < 0 |
495 */ |
495 */ |
496 |
496 |
497 int ec_slave_fetch_sii_pdos( |
497 int ec_slave_fetch_sii_pdos( |
498 ec_slave_t *slave, /**< EtherCAT slave */ |
498 ec_slave_t *slave, /**< EtherCAT slave */ |
499 const uint8_t *data, /**< category data */ |
499 const uint8_t *data, /**< category data */ |
500 size_t data_size, /**< number of bytes */ |
500 size_t data_size, /**< number of bytes */ |
501 ec_direction_t dir /**< PDO direction. */ |
501 ec_direction_t dir /**< Pdo direction. */ |
502 ) |
502 ) |
503 { |
503 { |
504 ec_pdo_t *pdo; |
504 ec_pdo_t *pdo; |
505 ec_pdo_entry_t *entry; |
505 ec_pdo_entry_t *entry; |
506 unsigned int entry_count, i; |
506 unsigned int entry_count, i; |
507 |
507 |
508 while (data_size >= 8) { |
508 while (data_size >= 8) { |
509 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
509 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
510 EC_ERR("Failed to allocate PDO memory.\n"); |
510 EC_ERR("Failed to allocate Pdo memory.\n"); |
511 return -1; |
511 return -1; |
512 } |
512 } |
513 |
513 |
514 ec_pdo_init(pdo); |
514 ec_pdo_init(pdo); |
515 pdo->dir = dir; |
515 pdo->dir = dir; |
527 data_size -= 8; |
527 data_size -= 8; |
528 data += 8; |
528 data += 8; |
529 |
529 |
530 for (i = 0; i < entry_count; i++) { |
530 for (i = 0; i < entry_count; i++) { |
531 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
531 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
532 EC_ERR("Failed to allocate PDO entry memory.\n"); |
532 EC_ERR("Failed to allocate Pdo entry memory.\n"); |
533 return -1; |
533 return -1; |
534 } |
534 } |
535 |
535 |
536 ec_pdo_entry_init(entry); |
536 ec_pdo_entry_init(entry); |
537 entry->index = EC_READ_U16(data); |
537 entry->index = EC_READ_U16(data); |
547 |
547 |
548 data_size -= 8; |
548 data_size -= 8; |
549 data += 8; |
549 data += 8; |
550 } |
550 } |
551 |
551 |
552 // if sync manager index is positive, the PDO is mapped by default |
552 // if sync manager index is positive, the Pdo is mapped by default |
553 if (pdo->sync_index >= 0) { |
553 if (pdo->sync_index >= 0) { |
554 ec_sync_t *sync; |
554 ec_sync_t *sync; |
555 |
555 |
556 if (pdo->sync_index >= slave->sii_sync_count) { |
556 if (pdo->sync_index >= slave->sii_sync_count) { |
557 EC_ERR("Invalid SM index %i for PDO 0x%04X in slave %u.", |
557 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.", |
558 pdo->sync_index, pdo->index, slave->ring_position); |
558 pdo->sync_index, pdo->index, slave->ring_position); |
559 return -1; |
559 return -1; |
560 } |
560 } |
561 sync = &slave->sii_syncs[pdo->sync_index]; |
561 sync = &slave->sii_syncs[pdo->sync_index]; |
562 |
562 |
720 buf += sprintf(buf, " Name: %s\n", slave->sii_name); |
720 buf += sprintf(buf, " Name: %s\n", slave->sii_name); |
721 buf += sprintf(buf, "\n"); |
721 buf += sprintf(buf, "\n"); |
722 } |
722 } |
723 |
723 |
724 if (slave->sii_sync_count) { |
724 if (slave->sii_sync_count) { |
725 buf += sprintf(buf, "Sync managers / PDO mapping:\n"); |
725 buf += sprintf(buf, "Sync managers / Pdo mapping:\n"); |
726 |
726 |
727 for (i = 0; i < slave->sii_sync_count; i++) { |
727 for (i = 0; i < slave->sii_sync_count; i++) { |
728 sync = &slave->sii_syncs[i]; |
728 sync = &slave->sii_syncs[i]; |
729 buf += sprintf(buf, |
729 buf += sprintf(buf, |
730 " SM%u: addr 0x%04X, size %u, control 0x%02X, %s\n", |
730 " SM%u: addr 0x%04X, size %u, control 0x%02X, %s\n", |
731 sync->index, sync->physical_start_address, |
731 sync->index, sync->physical_start_address, |
732 sync->length, sync->control_register, |
732 sync->length, sync->control_register, |
733 sync->enable ? "enable" : "disable"); |
733 sync->enable ? "enable" : "disable"); |
734 |
734 |
735 if (list_empty(&sync->mapping.pdos)) { |
735 if (list_empty(&sync->mapping.pdos)) { |
736 buf += sprintf(buf, " No PDOs mapped.\n"); |
736 buf += sprintf(buf, " No Pdos mapped.\n"); |
737 } else if (sync->mapping_source != EC_SYNC_MAPPING_NONE) { |
737 } else if (sync->mapping_source != EC_SYNC_MAPPING_NONE) { |
738 buf += sprintf(buf, |
738 buf += sprintf(buf, |
739 " PDO mapping information from %s.\n", |
739 " Pdo mapping information from %s.\n", |
740 sync->mapping_source == EC_SYNC_MAPPING_SII |
740 sync->mapping_source == EC_SYNC_MAPPING_SII |
741 ? "SII" : "CoE"); |
741 ? "SII" : "CoE"); |
742 } |
742 } |
743 |
743 |
744 list_for_each_entry(pdo, &sync->mapping.pdos, list) { |
744 list_for_each_entry(pdo, &sync->mapping.pdos, list) { |
758 buf += sprintf(buf, "\n"); |
758 buf += sprintf(buf, "\n"); |
759 } |
759 } |
760 |
760 |
761 // type-cast to avoid warnings on some compilers |
761 // type-cast to avoid warnings on some compilers |
762 if (!list_empty((struct list_head *) &slave->sii_pdos)) { |
762 if (!list_empty((struct list_head *) &slave->sii_pdos)) { |
763 buf += sprintf(buf, "Available PDOs from SII:\n"); |
763 buf += sprintf(buf, "Available Pdos from SII:\n"); |
764 |
764 |
765 list_for_each_entry(pdo, &slave->sii_pdos, list) { |
765 list_for_each_entry(pdo, &slave->sii_pdos, list) { |
766 buf += sprintf(buf, " %s 0x%04X \"%s\"", |
766 buf += sprintf(buf, " %s 0x%04X \"%s\"", |
767 pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo", |
767 pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo", |
768 pdo->index, pdo->name ? pdo->name : "???"); |
768 pdo->index, pdo->name ? pdo->name : "???"); |
1109 } |
1109 } |
1110 |
1110 |
1111 /*****************************************************************************/ |
1111 /*****************************************************************************/ |
1112 |
1112 |
1113 /** |
1113 /** |
1114 * Get the sync manager for either Rx- or Tx-PDOs. |
1114 * Get the sync manager for either Rx- or Tx-Pdos. |
1115 * \return pointer to sync manager, or NULL. |
1115 * \return pointer to sync manager, or NULL. |
1116 */ |
1116 */ |
1117 |
1117 |
1118 ec_sync_t *ec_slave_get_pdo_sync( |
1118 ec_sync_t *ec_slave_get_pdo_sync( |
1119 ec_slave_t *slave, /**< EtherCAT slave */ |
1119 ec_slave_t *slave, /**< EtherCAT slave */ |
1159 } |
1159 } |
1160 |
1160 |
1161 /*****************************************************************************/ |
1161 /*****************************************************************************/ |
1162 |
1162 |
1163 /** |
1163 /** |
1164 Counts the total number of SDOs and entries in the dictionary. |
1164 Counts the total number of Sdos and entries in the dictionary. |
1165 */ |
1165 */ |
1166 |
1166 |
1167 void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */ |
1167 void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */ |
1168 unsigned int *sdo_count, /**< number of SDOs */ |
1168 unsigned int *sdo_count, /**< number of Sdos */ |
1169 unsigned int *entry_count /**< total number of |
1169 unsigned int *entry_count /**< total number of |
1170 entries */ |
1170 entries */ |
1171 ) |
1171 ) |
1172 { |
1172 { |
1173 unsigned int sdos = 0, entries = 0; |
1173 unsigned int sdos = 0, entries = 0; |
1186 } |
1186 } |
1187 |
1187 |
1188 /*****************************************************************************/ |
1188 /*****************************************************************************/ |
1189 |
1189 |
1190 /** |
1190 /** |
1191 * Get an SDO from the dictionary. |
1191 * Get an Sdo from the dictionary. |
1192 * \returns The desired SDO, or NULL. |
1192 * \returns The desired Sdo, or NULL. |
1193 */ |
1193 */ |
1194 |
1194 |
1195 ec_sdo_t *ec_slave_get_sdo( |
1195 ec_sdo_t *ec_slave_get_sdo( |
1196 ec_slave_t *slave /**< EtherCAT slave */, |
1196 ec_slave_t *slave /**< EtherCAT slave */, |
1197 uint16_t index /**< SDO index */ |
1197 uint16_t index /**< Sdo index */ |
1198 ) |
1198 ) |
1199 { |
1199 { |
1200 ec_sdo_t *sdo; |
1200 ec_sdo_t *sdo; |
1201 |
1201 |
1202 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
1202 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |