85 slave->base_type = 0; |
85 slave->base_type = 0; |
86 slave->base_revision = 0; |
86 slave->base_revision = 0; |
87 slave->base_build = 0; |
87 slave->base_build = 0; |
88 slave->base_fmmu_count = 0; |
88 slave->base_fmmu_count = 0; |
89 |
89 |
90 for (i = 0; i < EC_SLAVE_MAX_PORTS; i++) { |
90 for (i = 0; i < EC_MAX_PORTS; i++) { |
91 slave->ports[i].dl_link = 0; |
91 slave->ports[i].dl_link = 0; |
92 slave->ports[i].dl_loop = 0; |
92 slave->ports[i].dl_loop = 0; |
93 slave->ports[i].dl_signal = 0; |
93 slave->ports[i].dl_signal = 0; |
94 slave->sii.physical_layer[i] = 0xFF; |
94 slave->sii.physical_layer[i] = 0xFF; |
95 } |
95 } |
363 |
363 |
364 count = data_size / 8; |
364 count = data_size / 8; |
365 |
365 |
366 if (count) { |
366 if (count) { |
367 total_count = count + slave->sii.sync_count; |
367 total_count = count + slave->sii.sync_count; |
|
368 if (total_count > EC_MAX_SYNCS) { |
|
369 EC_ERR("Exceeded maximum number of sync managers!\n"); |
|
370 return -1; |
|
371 } |
368 memsize = sizeof(ec_sync_t) * total_count; |
372 memsize = sizeof(ec_sync_t) * total_count; |
369 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
373 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
370 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
374 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
371 memsize); |
375 memsize); |
372 return -1; |
376 return -1; |
378 // initialize new sync managers |
382 // initialize new sync managers |
379 for (i = 0; i < count; i++, data += 8) { |
383 for (i = 0; i < count; i++, data += 8) { |
380 index = i + slave->sii.sync_count; |
384 index = i + slave->sii.sync_count; |
381 sync = &syncs[index]; |
385 sync = &syncs[index]; |
382 |
386 |
383 ec_sync_init(sync, slave, index); |
387 ec_sync_init(sync, slave); |
384 sync->physical_start_address = EC_READ_U16(data); |
388 sync->physical_start_address = EC_READ_U16(data); |
385 sync->length = EC_READ_U16(data + 2); |
389 sync->default_length = EC_READ_U16(data + 2); |
386 sync->control_register = EC_READ_U8(data + 4); |
390 sync->control_register = EC_READ_U8(data + 4); |
387 sync->enable = EC_READ_U8(data + 6); |
391 sync->enable = EC_READ_U8(data + 6); |
388 } |
392 } |
389 |
393 |
390 if (slave->sii.syncs) |
394 if (slave->sii.syncs) |
419 EC_ERR("Failed to allocate Pdo memory.\n"); |
423 EC_ERR("Failed to allocate Pdo memory.\n"); |
420 return -1; |
424 return -1; |
421 } |
425 } |
422 |
426 |
423 ec_pdo_init(pdo); |
427 ec_pdo_init(pdo); |
424 pdo->dir = dir; |
|
425 pdo->index = EC_READ_U16(data); |
428 pdo->index = EC_READ_U16(data); |
426 entry_count = EC_READ_U8(data + 2); |
429 entry_count = EC_READ_U8(data + 2); |
427 pdo->sync_index = EC_READ_U8(data + 3); |
430 pdo->sync_index = EC_READ_U8(data + 3); |
428 if (ec_pdo_set_name(pdo, |
431 if (ec_pdo_set_name(pdo, |
429 ec_slave_sii_string(slave, EC_READ_U8(data + 5)))) { |
432 ec_slave_sii_string(slave, EC_READ_U8(data + 5)))) { |
460 |
463 |
461 // if sync manager index is positive, the Pdo is mapped by default |
464 // if sync manager index is positive, the Pdo is mapped by default |
462 if (pdo->sync_index >= 0) { |
465 if (pdo->sync_index >= 0) { |
463 ec_sync_t *sync; |
466 ec_sync_t *sync; |
464 |
467 |
465 if (pdo->sync_index >= slave->sii.sync_count) { |
468 if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) { |
466 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.", |
469 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.", |
467 pdo->sync_index, pdo->index, slave->ring_position); |
470 pdo->sync_index, pdo->index, slave->ring_position); |
468 return -1; |
471 return -1; |
469 } |
472 } |
470 sync = &slave->sii.syncs[pdo->sync_index]; |
|
471 |
473 |
472 if (ec_pdo_list_add_pdo_copy(&sync->pdos, pdo)) |
474 if (ec_pdo_list_add_pdo_copy(&sync->pdos, pdo)) |
473 return -1; |
475 return -1; |
474 |
476 |
475 sync->assign_source = EC_ASSIGN_SII; |
477 sync->assign_source = EC_ASSIGN_SII; |
562 } |
564 } |
563 } |
565 } |
564 |
566 |
565 /*****************************************************************************/ |
567 /*****************************************************************************/ |
566 |
568 |
567 /** Get the sync manager for either Rx- or Tx-Pdos. |
569 /** Get the sync manager given an index. |
568 * |
570 * |
569 * \todo This seems not to be correct in every case... |
|
570 * \return pointer to sync manager, or NULL. |
571 * \return pointer to sync manager, or NULL. |
571 */ |
572 */ |
572 ec_sync_t *ec_slave_get_pdo_sync( |
573 ec_sync_t *ec_slave_get_sync( |
573 ec_slave_t *slave, /**< EtherCAT slave. */ |
574 ec_slave_t *slave, /**< EtherCAT slave. */ |
574 ec_direction_t dir /**< Input or output. */ |
575 uint8_t sync_index /**< Sync manager index. */ |
575 ) |
576 ) |
576 { |
577 { |
577 unsigned int sync_index; |
578 if (sync_index < slave->sii.sync_count) { |
578 |
579 return &slave->sii.syncs[sync_index]; |
579 if (dir != EC_DIR_INPUT && dir != EC_DIR_OUTPUT) { |
580 } else { |
580 EC_ERR("Invalid direction!\n"); |
|
581 return NULL; |
581 return NULL; |
582 } |
582 } |
583 |
|
584 if (slave->sii.sync_count != 1) { |
|
585 sync_index = (unsigned int) dir; |
|
586 if (slave->sii.mailbox_protocols) sync_index += 2; |
|
587 |
|
588 if (sync_index >= slave->sii.sync_count) |
|
589 return NULL; |
|
590 } else { // sync_count == 1 |
|
591 // A single sync manager may be used for inputs OR outputs! |
|
592 if (ec_sync_direction(&slave->sii.syncs[0]) != dir) |
|
593 return NULL; |
|
594 sync_index = 0; |
|
595 } |
|
596 |
|
597 return &slave->sii.syncs[sync_index]; |
|
598 } |
583 } |
599 |
584 |
600 /*****************************************************************************/ |
585 /*****************************************************************************/ |
601 |
586 |
602 /** |
587 /** |