125 |
125 |
126 slave->base_type = 0; |
126 slave->base_type = 0; |
127 slave->base_revision = 0; |
127 slave->base_revision = 0; |
128 slave->base_build = 0; |
128 slave->base_build = 0; |
129 slave->base_fmmu_count = 0; |
129 slave->base_fmmu_count = 0; |
130 slave->base_sync_count = 0; |
|
131 |
130 |
132 slave->eeprom_data = NULL; |
131 slave->eeprom_data = NULL; |
133 slave->eeprom_size = 0; |
132 slave->eeprom_size = 0; |
134 |
133 |
135 slave->sii_alias = 0; |
134 slave->sii_alias = 0; |
147 slave->sii_order = NULL; |
146 slave->sii_order = NULL; |
148 slave->sii_name = NULL; |
147 slave->sii_name = NULL; |
149 slave->sii_current_on_ebus = 0; |
148 slave->sii_current_on_ebus = 0; |
150 |
149 |
151 INIT_LIST_HEAD(&slave->sii_strings); |
150 INIT_LIST_HEAD(&slave->sii_strings); |
152 INIT_LIST_HEAD(&slave->sii_syncs); |
151 slave->sii_syncs = NULL; |
|
152 slave->sii_sync_count = 0; |
153 INIT_LIST_HEAD(&slave->sii_pdos); |
153 INIT_LIST_HEAD(&slave->sii_pdos); |
154 INIT_LIST_HEAD(&slave->sdo_dictionary); |
154 INIT_LIST_HEAD(&slave->sdo_dictionary); |
155 INIT_LIST_HEAD(&slave->sdo_confs); |
155 INIT_LIST_HEAD(&slave->sdo_confs); |
156 |
156 |
157 slave->sdo_dictionary_fetched = 0; |
157 slave->sdo_dictionary_fetched = 0; |
238 |
238 |
239 void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */) |
239 void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */) |
240 { |
240 { |
241 ec_slave_t *slave; |
241 ec_slave_t *slave; |
242 ec_sii_string_t *string, *next_str; |
242 ec_sii_string_t *string, *next_str; |
243 ec_sii_sync_t *sync, *next_sync; |
|
244 ec_sii_pdo_t *pdo, *next_pdo; |
243 ec_sii_pdo_t *pdo, *next_pdo; |
245 ec_sii_pdo_entry_t *entry, *next_ent; |
244 ec_sii_pdo_entry_t *entry, *next_ent; |
246 ec_sdo_data_t *sdodata, *next_sdodata; |
245 ec_sdo_data_t *sdodata, *next_sdodata; |
247 |
246 |
248 slave = container_of(kobj, ec_slave_t, kobj); |
247 slave = container_of(kobj, ec_slave_t, kobj); |
252 list_del(&string->list); |
251 list_del(&string->list); |
253 kfree(string); |
252 kfree(string); |
254 } |
253 } |
255 |
254 |
256 // free all sync managers |
255 // free all sync managers |
257 list_for_each_entry_safe(sync, next_sync, &slave->sii_syncs, list) { |
256 if (slave->sii_syncs) kfree(slave->sii_syncs); |
258 list_del(&sync->list); |
|
259 kfree(sync); |
|
260 } |
|
261 |
257 |
262 // free all PDOs |
258 // free all PDOs |
263 list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { |
259 list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { |
264 list_del(&pdo->list); |
260 list_del(&pdo->list); |
265 if (pdo->name) kfree(pdo->name); |
261 if (pdo->name) kfree(pdo->name); |
307 */ |
303 */ |
308 |
304 |
309 void ec_slave_reset(ec_slave_t *slave /**< EtherCAT slave */) |
305 void ec_slave_reset(ec_slave_t *slave /**< EtherCAT slave */) |
310 { |
306 { |
311 ec_sdo_data_t *sdodata, *next_sdodata; |
307 ec_sdo_data_t *sdodata, *next_sdodata; |
312 ec_sii_sync_t *sync; |
308 unsigned int i; |
313 |
309 |
314 // remove FMMU configurations |
310 // remove FMMU configurations |
315 slave->fmmu_count = 0; |
311 slave->fmmu_count = 0; |
316 slave->pdos_registered = 0; |
312 slave->pdos_registered = 0; |
317 |
313 |
321 kfree(sdodata->data); |
317 kfree(sdodata->data); |
322 kfree(sdodata); |
318 kfree(sdodata); |
323 } |
319 } |
324 |
320 |
325 // remove estimated sync manager sizes |
321 // remove estimated sync manager sizes |
326 list_for_each_entry(sync, &slave->sii_syncs, list) { |
322 for (i = 0; i < slave->sii_sync_count; i++) { |
327 sync->est_length = 0; |
323 slave->sii_syncs[i].est_length = 0; |
328 } |
324 } |
329 } |
325 } |
330 |
326 |
331 /*****************************************************************************/ |
327 /*****************************************************************************/ |
332 |
328 |
470 int ec_slave_fetch_sync(ec_slave_t *slave, /**< EtherCAT slave */ |
466 int ec_slave_fetch_sync(ec_slave_t *slave, /**< EtherCAT slave */ |
471 const uint8_t *data, /**< category data */ |
467 const uint8_t *data, /**< category data */ |
472 size_t word_count /**< number of words */ |
468 size_t word_count /**< number of words */ |
473 ) |
469 ) |
474 { |
470 { |
475 unsigned int sync_count, i; |
471 unsigned int i; |
476 ec_sii_sync_t *sync; |
472 ec_sii_sync_t *sync; |
477 |
473 |
478 sync_count = word_count / 4; // sync manager struct is 4 words long |
474 // sync manager struct is 4 words long |
479 |
475 slave->sii_sync_count = word_count / 4; |
480 for (i = 0; i < sync_count; i++, data += 8) { |
476 |
481 if (!(sync = (ec_sii_sync_t *) |
477 if (!(slave->sii_syncs = kmalloc(sizeof(ec_sii_sync_t) * |
482 kmalloc(sizeof(ec_sii_sync_t), GFP_ATOMIC))) { |
478 slave->sii_sync_count, GFP_ATOMIC))) { |
483 EC_ERR("Failed to allocate Sync-Manager memory.\n"); |
479 EC_ERR("Failed to allocate Sync-Manager memory.\n"); |
484 return -1; |
480 return -1; |
485 } |
481 } |
486 |
482 |
487 sync->index = i; |
483 for (i = 0; i < slave->sii_sync_count; i++, data += 8) { |
|
484 sync = &slave->sii_syncs[i]; |
|
485 |
|
486 sync->index = i; |
488 sync->physical_start_address = EC_READ_U16(data); |
487 sync->physical_start_address = EC_READ_U16(data); |
489 sync->length = EC_READ_U16(data + 2); |
488 sync->length = EC_READ_U16(data + 2); |
490 sync->control_register = EC_READ_U8 (data + 4); |
489 sync->control_register = EC_READ_U8 (data + 4); |
491 sync->enable = EC_READ_U8 (data + 6); |
490 sync->enable = EC_READ_U8 (data + 6); |
492 |
491 |
493 sync->est_length = 0; |
492 sync->est_length = 0; |
494 |
|
495 list_add_tail(&sync->list, &slave->sii_syncs); |
|
496 } |
493 } |
497 |
494 |
498 return 0; |
495 return 0; |
499 } |
496 } |
500 |
497 |
783 if (slave->sii_image) |
780 if (slave->sii_image) |
784 off += sprintf(buffer + off, " Image: %s\n", slave->sii_image); |
781 off += sprintf(buffer + off, " Image: %s\n", slave->sii_image); |
785 if (slave->sii_order) |
782 if (slave->sii_order) |
786 off += sprintf(buffer + off, " Order number: %s\n", slave->sii_order); |
783 off += sprintf(buffer + off, " Order number: %s\n", slave->sii_order); |
787 |
784 |
788 if (!list_empty(&slave->sii_syncs)) |
785 if (slave->sii_sync_count) |
789 off += sprintf(buffer + off, "\nSync-Managers:\n"); |
786 off += sprintf(buffer + off, "\nSync-Managers:\n"); |
790 |
787 |
791 list_for_each_entry(sync, &slave->sii_syncs, list) { |
788 for (i = 0; i < slave->sii_sync_count; i++) { |
|
789 sync = &slave->sii_syncs[i]; |
792 off += sprintf(buffer + off, " %i: 0x%04X, length %i," |
790 off += sprintf(buffer + off, " %i: 0x%04X, length %i," |
793 " control 0x%02X, %s\n", |
791 " control 0x%02X, %s\n", |
794 sync->index, sync->physical_start_address, |
792 sync->index, sync->physical_start_address, |
795 sync->length, sync->control_register, |
793 sync->length, sync->control_register, |
796 sync->enable ? "enable" : "disable"); |
794 sync->enable ? "enable" : "disable"); |
797 } |
795 } |
798 |
796 |
799 if (!list_empty(&slave->sii_pdos)) |
797 if (!list_empty(&slave->sii_pdos)) |
800 off += sprintf(buffer + off, "\nPDOs:\n"); |
798 off += sprintf(buffer + off, "\nPDOs:\n"); |
801 |
799 |