386 |
386 |
387 /*****************************************************************************/ |
387 /*****************************************************************************/ |
388 |
388 |
389 /** |
389 /** |
390 Fetches data from a STRING category. |
390 Fetches data from a STRING category. |
|
391 \todo range checking |
391 \return 0 in case of success, else < 0 |
392 \return 0 in case of success, else < 0 |
392 */ |
393 */ |
393 |
394 |
394 int ec_slave_fetch_sii_strings( |
395 int ec_slave_fetch_sii_strings( |
395 ec_slave_t *slave, /**< EtherCAT slave */ |
396 ec_slave_t *slave, /**< EtherCAT slave */ |
396 const uint8_t *data /**< category data */ |
397 const uint8_t *data, /**< category data */ |
|
398 size_t data_size /**< number of bytes */ |
397 ) |
399 ) |
398 { |
400 { |
399 int i; |
401 int i; |
400 size_t size; |
402 size_t size; |
401 off_t offset; |
403 off_t offset; |
442 /** |
444 /** |
443 Fetches data from a GENERAL category. |
445 Fetches data from a GENERAL category. |
444 \return 0 in case of success, else < 0 |
446 \return 0 in case of success, else < 0 |
445 */ |
447 */ |
446 |
448 |
447 void ec_slave_fetch_sii_general( |
449 int ec_slave_fetch_sii_general( |
448 ec_slave_t *slave, /**< EtherCAT slave */ |
450 ec_slave_t *slave, /**< EtherCAT slave */ |
449 const uint8_t *data /**< category data */ |
451 const uint8_t *data, /**< category data */ |
|
452 size_t data_size /**< size in bytes */ |
450 ) |
453 ) |
451 { |
454 { |
452 unsigned int i; |
455 unsigned int i; |
|
456 |
|
457 if (data_size != 32) { |
|
458 EC_ERR("Wrong size of general category (%u/32) in slave %u.\n", |
|
459 data_size, slave->ring_position); |
|
460 return -1; |
|
461 } |
453 |
462 |
454 slave->sii_group = ec_slave_sii_string(slave, data[0]); |
463 slave->sii_group = ec_slave_sii_string(slave, data[0]); |
455 slave->sii_image = ec_slave_sii_string(slave, data[1]); |
464 slave->sii_image = ec_slave_sii_string(slave, data[1]); |
456 slave->sii_order = ec_slave_sii_string(slave, data[2]); |
465 slave->sii_order = ec_slave_sii_string(slave, data[2]); |
457 slave->sii_name = ec_slave_sii_string(slave, data[3]); |
466 slave->sii_name = ec_slave_sii_string(slave, data[3]); |
459 for (i = 0; i < 4; i++) |
468 for (i = 0; i < 4; i++) |
460 slave->sii_physical_layer[i] = |
469 slave->sii_physical_layer[i] = |
461 (data[4] & (0x03 << (i * 2))) >> (i * 2); |
470 (data[4] & (0x03 << (i * 2))) >> (i * 2); |
462 |
471 |
463 slave->sii_current_on_ebus = EC_READ_S16(data + 0x0C); |
472 slave->sii_current_on_ebus = EC_READ_S16(data + 0x0C); |
|
473 |
|
474 return 0; |
464 } |
475 } |
465 |
476 |
466 /*****************************************************************************/ |
477 /*****************************************************************************/ |
467 |
478 |
468 /** |
479 /** |
471 */ |
482 */ |
472 |
483 |
473 int ec_slave_fetch_sii_syncs( |
484 int ec_slave_fetch_sii_syncs( |
474 ec_slave_t *slave, /**< EtherCAT slave */ |
485 ec_slave_t *slave, /**< EtherCAT slave */ |
475 const uint8_t *data, /**< category data */ |
486 const uint8_t *data, /**< category data */ |
476 size_t word_count /**< number of words */ |
487 size_t data_size /**< number of bytes */ |
477 ) |
488 ) |
478 { |
489 { |
479 unsigned int i; |
490 unsigned int i; |
480 ec_sync_t *sync; |
491 ec_sync_t *sync; |
481 |
492 size_t memsize; |
482 // sync manager struct is 4 words long |
493 |
483 slave->sii_sync_count = word_count / 4; |
494 // one sync manager struct is 4 words long |
484 |
495 if (data_size % 8) { |
485 if (!(slave->sii_syncs = |
496 EC_ERR("Invalid SII sync manager size %u in slave %u.\n", |
486 kmalloc(sizeof(ec_sync_t) * slave->sii_sync_count, |
497 data_size, slave->ring_position); |
487 GFP_KERNEL))) { |
498 return -1; |
488 EC_ERR("Failed to allocate memory for sync managers.\n"); |
499 } |
|
500 |
|
501 slave->sii_sync_count = data_size / 8; |
|
502 |
|
503 memsize = sizeof(ec_sync_t) * slave->sii_sync_count; |
|
504 if (!(slave->sii_syncs = kmalloc(memsize, GFP_KERNEL))) { |
|
505 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
|
506 memsize); |
489 slave->sii_sync_count = 0; |
507 slave->sii_sync_count = 0; |
490 return -1; |
508 return -1; |
491 } |
509 } |
492 |
510 |
493 for (i = 0; i < slave->sii_sync_count; i++, data += 8) { |
511 for (i = 0; i < slave->sii_sync_count; i++, data += 8) { |
511 */ |
529 */ |
512 |
530 |
513 int ec_slave_fetch_sii_pdos( |
531 int ec_slave_fetch_sii_pdos( |
514 ec_slave_t *slave, /**< EtherCAT slave */ |
532 ec_slave_t *slave, /**< EtherCAT slave */ |
515 const uint8_t *data, /**< category data */ |
533 const uint8_t *data, /**< category data */ |
516 size_t word_count, /**< number of words */ |
534 size_t data_size, /**< number of bytes */ |
517 ec_pdo_type_t pdo_type /**< PDO type */ |
535 ec_pdo_type_t pdo_type /**< PDO type */ |
518 ) |
536 ) |
519 { |
537 { |
520 ec_pdo_t *pdo; |
538 ec_pdo_t *pdo; |
521 ec_pdo_entry_t *entry; |
539 ec_pdo_entry_t *entry; |
522 unsigned int entry_count, i; |
540 unsigned int entry_count, i; |
523 |
541 |
524 while (word_count >= 4) { |
542 while (data_size >= 8) { |
525 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
543 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
526 EC_ERR("Failed to allocate PDO memory.\n"); |
544 EC_ERR("Failed to allocate PDO memory.\n"); |
527 return -1; |
545 return -1; |
528 } |
546 } |
529 |
547 |
533 entry_count = EC_READ_U8(data + 2); |
551 entry_count = EC_READ_U8(data + 2); |
534 pdo->sync_index = EC_READ_U8(data + 3); |
552 pdo->sync_index = EC_READ_U8(data + 3); |
535 pdo->name = ec_slave_sii_string(slave, EC_READ_U8(data + 5)); |
553 pdo->name = ec_slave_sii_string(slave, EC_READ_U8(data + 5)); |
536 list_add_tail(&pdo->list, &slave->sii_pdos); |
554 list_add_tail(&pdo->list, &slave->sii_pdos); |
537 |
555 |
538 word_count -= 4; |
556 data_size -= 8; |
539 data += 8; |
557 data += 8; |
540 |
558 |
541 for (i = 0; i < entry_count; i++) { |
559 for (i = 0; i < entry_count; i++) { |
542 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
560 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
543 EC_ERR("Failed to allocate PDO entry memory.\n"); |
561 EC_ERR("Failed to allocate PDO entry memory.\n"); |
548 entry->subindex = EC_READ_U8(data + 2); |
566 entry->subindex = EC_READ_U8(data + 2); |
549 entry->name = ec_slave_sii_string(slave, EC_READ_U8(data + 3)); |
567 entry->name = ec_slave_sii_string(slave, EC_READ_U8(data + 3)); |
550 entry->bit_length = EC_READ_U8(data + 5); |
568 entry->bit_length = EC_READ_U8(data + 5); |
551 list_add_tail(&entry->list, &pdo->entries); |
569 list_add_tail(&entry->list, &pdo->entries); |
552 |
570 |
553 word_count -= 4; |
571 data_size -= 8; |
554 data += 8; |
572 data += 8; |
555 } |
573 } |
556 |
574 |
557 // if sync manager index is positive, the PDO is mapped by default |
575 // if sync manager index is positive, the PDO is mapped by default |
558 if (pdo->sync_index >= 0) { |
576 if (pdo->sync_index >= 0) { |