259 kfree(slave->sii.strings[i]); |
259 kfree(slave->sii.strings[i]); |
260 kfree(slave->sii.strings); |
260 kfree(slave->sii.strings); |
261 } |
261 } |
262 |
262 |
263 // free all sync managers |
263 // free all sync managers |
264 if (slave->sii.syncs) { |
264 ec_slave_clear_sync_managers(slave); |
265 for (i = 0; i < slave->sii.sync_count; i++) { |
|
266 ec_sync_clear(&slave->sii.syncs[i]); |
|
267 } |
|
268 kfree(slave->sii.syncs); |
|
269 } |
|
270 |
265 |
271 // free all SII Pdos |
266 // free all SII Pdos |
272 list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) { |
267 list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) { |
273 list_del(&pdo->list); |
268 list_del(&pdo->list); |
274 ec_pdo_clear(pdo); |
269 ec_pdo_clear(pdo); |
448 return 0; |
460 return 0; |
449 } |
461 } |
450 |
462 |
451 /*****************************************************************************/ |
463 /*****************************************************************************/ |
452 |
464 |
453 /** |
465 /** Fetches data from a SYNC MANAGER category. |
454 Fetches data from a SYNC MANAGER category. |
466 * |
455 \return 0 in case of success, else < 0 |
467 * Appends the sync managers described in the category to the existing ones. |
456 */ |
468 * |
457 |
469 * \return 0 in case of success, else < 0 |
|
470 */ |
458 int ec_slave_fetch_sii_syncs( |
471 int ec_slave_fetch_sii_syncs( |
459 ec_slave_t *slave, /**< EtherCAT slave */ |
472 ec_slave_t *slave, /**< EtherCAT slave. */ |
460 const uint8_t *data, /**< category data */ |
473 const uint8_t *data, /**< Category data. */ |
461 size_t data_size /**< number of bytes */ |
474 size_t data_size /**< Number of bytes. */ |
462 ) |
475 ) |
463 { |
476 { |
464 unsigned int i; |
477 unsigned int i, count, total_count; |
465 ec_sync_t *sync; |
478 ec_sync_t *sync; |
466 size_t memsize; |
479 size_t memsize; |
|
480 ec_sync_t *syncs; |
|
481 uint8_t index; |
467 |
482 |
468 // one sync manager struct is 4 words long |
483 // one sync manager struct is 4 words long |
469 if (data_size % 8) { |
484 if (data_size % 8) { |
470 EC_ERR("Invalid SII sync manager size %u in slave %u.\n", |
485 EC_ERR("Invalid SII sync manager category size %u in slave %u.\n", |
471 data_size, slave->ring_position); |
486 data_size, slave->ring_position); |
472 return -1; |
487 return -1; |
473 } |
488 } |
474 |
489 |
475 slave->sii.sync_count = data_size / 8; |
490 count = data_size / 8; |
476 |
491 |
477 memsize = sizeof(ec_sync_t) * slave->sii.sync_count; |
492 if (slave->master->debug_level) |
478 if (!(slave->sii.syncs = kmalloc(memsize, GFP_KERNEL))) { |
493 EC_DBG("Found Sync manager category with %u sync managers.\n", count); |
479 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
|
480 memsize); |
|
481 slave->sii.sync_count = 0; |
|
482 return -1; |
|
483 } |
|
484 |
494 |
485 for (i = 0; i < slave->sii.sync_count; i++, data += 8) { |
495 if (count) { |
486 sync = &slave->sii.syncs[i]; |
496 total_count = count + slave->sii.sync_count; |
487 |
497 memsize = sizeof(ec_sync_t) * total_count; |
488 ec_sync_init(sync, slave, i); |
498 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
489 sync->physical_start_address = EC_READ_U16(data); |
499 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
490 sync->length = EC_READ_U16(data + 2); |
500 memsize); |
491 sync->control_register = EC_READ_U8 (data + 4); |
501 return -1; |
492 sync->enable = EC_READ_U8 (data + 6); |
502 } |
493 } |
503 |
|
504 // copy existing sync managers |
|
505 memcpy(syncs, slave->sii.syncs, |
|
506 slave->sii.sync_count * sizeof(ec_sync_t)); |
|
507 |
|
508 // initialize new sync managers |
|
509 for (i = 0; i < count; i++, data += 8) { |
|
510 index = i + slave->sii.sync_count; |
|
511 sync = &syncs[index]; |
|
512 |
|
513 ec_sync_init(sync, slave, index); |
|
514 sync->physical_start_address = EC_READ_U16(data); |
|
515 sync->length = EC_READ_U16(data + 2); |
|
516 sync->control_register = EC_READ_U8(data + 4); |
|
517 sync->enable = EC_READ_U8(data + 6); |
|
518 } |
|
519 |
|
520 if (slave->sii.syncs) |
|
521 kfree(slave->sii.syncs); |
|
522 slave->sii.syncs = syncs; |
|
523 slave->sii.sync_count = total_count; |
|
524 } |
|
525 |
|
526 if (slave->master->debug_level) |
|
527 EC_DBG("Total sync managers: %u.\n", slave->sii.sync_count); |
494 |
528 |
495 return 0; |
529 return 0; |
496 } |
530 } |
497 |
531 |
498 /*****************************************************************************/ |
532 /*****************************************************************************/ |