57 ec_domain_t *domain, /**< EtherCAT domain. */ |
57 ec_domain_t *domain, /**< EtherCAT domain. */ |
58 ec_master_t *master, /**< Parent master. */ |
58 ec_master_t *master, /**< Parent master. */ |
59 unsigned int index /**< Index. */ |
59 unsigned int index /**< Index. */ |
60 ) |
60 ) |
61 { |
61 { |
|
62 unsigned int dev_idx; |
|
63 |
62 domain->master = master; |
64 domain->master = master; |
63 domain->index = index; |
65 domain->index = index; |
64 INIT_LIST_HEAD(&domain->fmmu_configs); |
66 INIT_LIST_HEAD(&domain->fmmu_configs); |
65 domain->data_size = 0; |
67 domain->data_size = 0; |
66 domain->data = NULL; |
68 domain->data = NULL; |
67 domain->data_origin = EC_ORIG_INTERNAL; |
69 domain->data_origin = EC_ORIG_INTERNAL; |
68 domain->logical_base_address = 0x00000000; |
70 domain->logical_base_address = 0x00000000; |
69 INIT_LIST_HEAD(&domain->datagram_pairs); |
71 INIT_LIST_HEAD(&domain->datagram_pairs); |
70 domain->working_counter[EC_DEVICE_MAIN] = 0x0000; |
72 for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); |
71 domain->working_counter[EC_DEVICE_BACKUP] = 0x0000; |
73 dev_idx++) { |
|
74 domain->working_counter[dev_idx] = 0x0000; |
|
75 } |
72 domain->expected_working_counter = 0x0000; |
76 domain->expected_working_counter = 0x0000; |
73 domain->working_counter_changes = 0; |
77 domain->working_counter_changes = 0; |
74 domain->redundancy_active = 0; |
78 domain->redundancy_active = 0; |
75 domain->notify_jiffies = 0; |
79 domain->notify_jiffies = 0; |
76 } |
80 } |
441 |
449 |
442 /*****************************************************************************/ |
450 /*****************************************************************************/ |
443 |
451 |
444 void ecrt_domain_process(ec_domain_t *domain) |
452 void ecrt_domain_process(ec_domain_t *domain) |
445 { |
453 { |
446 uint16_t wc_sum[EC_NUM_DEVICES] = {}; |
454 uint16_t wc_sum[EC_MAX_NUM_DEVICES] = {}, redundant_wc, wc_total; |
447 ec_datagram_pair_t *pair; |
455 ec_datagram_pair_t *pair; |
448 ec_datagram_t *main_datagram, *backup_datagram; |
456 ec_datagram_t *main_datagram; |
449 uint32_t logical_datagram_address; |
457 uint32_t logical_datagram_address; |
450 size_t datagram_size; |
458 size_t datagram_size; |
451 uint16_t datagram_pair_wc; |
459 uint16_t datagram_pair_wc; |
452 unsigned int datagram_offset; |
460 unsigned int dev_idx, wc_change; |
|
461 #if EC_MAX_NUM_DEVICES > 1 |
453 ec_fmmu_config_t *fmmu = |
462 ec_fmmu_config_t *fmmu = |
454 list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list); |
463 list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list); |
|
464 #endif |
455 unsigned int redundancy; |
465 unsigned int redundancy; |
456 |
466 |
457 #if DEBUG_REDUNDANCY |
467 #if DEBUG_REDUNDANCY |
458 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
468 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
459 #endif |
469 #endif |
460 |
470 |
461 list_for_each_entry(pair, &domain->datagram_pairs, list) { |
471 list_for_each_entry(pair, &domain->datagram_pairs, list) { |
462 |
472 |
463 main_datagram = &pair->datagrams[EC_DEVICE_MAIN]; |
473 main_datagram = &pair->datagrams[EC_DEVICE_MAIN]; |
464 backup_datagram = &pair->datagrams[EC_DEVICE_BACKUP]; |
|
465 logical_datagram_address = EC_READ_U32(main_datagram->address); |
474 logical_datagram_address = EC_READ_U32(main_datagram->address); |
466 datagram_size = main_datagram->data_size; |
475 datagram_size = main_datagram->data_size; |
467 |
476 |
468 #if DEBUG_REDUNDANCY |
477 #if DEBUG_REDUNDANCY |
469 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
478 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
470 main_datagram->name, logical_datagram_address); |
479 main_datagram->name, logical_datagram_address); |
471 #endif |
480 #endif |
472 |
481 |
473 datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum); |
482 datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum); |
474 |
483 |
475 /* Go through all FMMU configs to detect data changes. */ |
484 #if EC_MAX_NUM_DEVICES > 1 |
|
485 if (ec_master_num_devices(domain->master) < 2) { |
|
486 continue; |
|
487 } |
|
488 |
|
489 /* Redundancy: Go through all FMMU configs to detect data changes. */ |
476 list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) { |
490 list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) { |
|
491 unsigned int datagram_offset; |
|
492 ec_datagram_t *backup_datagram = |
|
493 &pair->datagrams[EC_DEVICE_BACKUP]; |
477 |
494 |
478 if (fmmu->dir != EC_DIR_INPUT) { |
495 if (fmmu->dir != EC_DIR_INPUT) { |
479 continue; |
496 continue; |
480 } |
497 } |
481 |
498 |
531 EC_MASTER_DBG(domain->master, 1, |
548 EC_MASTER_DBG(domain->master, 1, |
532 "no change and incomplete\n"); |
549 "no change and incomplete\n"); |
533 #endif |
550 #endif |
534 } |
551 } |
535 } |
552 } |
536 } |
553 #endif // EC_MAX_NUM_DEVICES > 1 |
537 |
554 } |
538 redundancy = wc_sum[EC_DEVICE_BACKUP] > 0; |
555 |
|
556 redundant_wc = 0; |
|
557 for (dev_idx = EC_DEVICE_BACKUP; |
|
558 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
559 redundant_wc += wc_sum[dev_idx]; |
|
560 } |
|
561 |
|
562 redundancy = redundant_wc > 0; |
539 if (redundancy != domain->redundancy_active) { |
563 if (redundancy != domain->redundancy_active) { |
540 if (redundancy) { |
564 if (redundancy) { |
541 EC_MASTER_WARN(domain->master, |
565 EC_MASTER_WARN(domain->master, |
542 "Domain %u: Redundant link in use!\n", |
566 "Domain %u: Redundant link in use!\n", |
543 domain->index); |
567 domain->index); |
547 domain->index); |
571 domain->index); |
548 } |
572 } |
549 domain->redundancy_active = redundancy; |
573 domain->redundancy_active = redundancy; |
550 } |
574 } |
551 |
575 |
552 if ((wc_sum[EC_DEVICE_MAIN] != domain->working_counter[EC_DEVICE_MAIN]) |
576 wc_change = 0; |
553 || (wc_sum[EC_DEVICE_BACKUP] |
577 wc_total = 0; |
554 != domain->working_counter[EC_DEVICE_BACKUP])) { |
578 for (dev_idx = EC_DEVICE_MAIN; |
|
579 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
580 if (wc_sum[dev_idx] != domain->working_counter[dev_idx]) { |
|
581 wc_change = 1; |
|
582 domain->working_counter[dev_idx] = wc_sum[dev_idx]; |
|
583 } |
|
584 wc_total += wc_sum[dev_idx]; |
|
585 } |
|
586 |
|
587 if (wc_change) { |
555 domain->working_counter_changes++; |
588 domain->working_counter_changes++; |
556 domain->working_counter[EC_DEVICE_MAIN] = wc_sum[EC_DEVICE_MAIN]; |
|
557 domain->working_counter[EC_DEVICE_BACKUP] = wc_sum[EC_DEVICE_BACKUP]; |
|
558 } |
589 } |
559 |
590 |
560 if (domain->working_counter_changes && |
591 if (domain->working_counter_changes && |
561 jiffies - domain->notify_jiffies > HZ) { |
592 jiffies - domain->notify_jiffies > HZ) { |
562 domain->notify_jiffies = jiffies; |
593 domain->notify_jiffies = jiffies; |
563 if (domain->working_counter_changes == 1) { |
594 if (domain->working_counter_changes == 1) { |
564 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
595 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
565 " changed to %u/%u (%u+%u).\n", domain->index, |
596 " changed to %u/%u", domain->index, |
566 domain->working_counter[EC_DEVICE_MAIN] + |
597 wc_total, domain->expected_working_counter); |
567 domain->working_counter[EC_DEVICE_BACKUP], |
|
568 domain->expected_working_counter, |
|
569 wc_sum[EC_DEVICE_MAIN], wc_sum[EC_DEVICE_BACKUP]); |
|
570 } else { |
598 } else { |
571 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
599 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
572 " changes - now %u/%u (%u+%u).\n", domain->index, |
600 " changes - now %u/%u", domain->index, |
573 domain->working_counter_changes, |
601 domain->working_counter_changes, |
574 domain->working_counter[EC_DEVICE_MAIN] + |
602 wc_total, domain->expected_working_counter); |
575 domain->working_counter[EC_DEVICE_BACKUP], |
603 } |
576 domain->expected_working_counter, |
604 if (ec_master_num_devices(domain->master) > 1) { |
577 wc_sum[EC_DEVICE_MAIN], wc_sum[EC_DEVICE_BACKUP]); |
605 printk(" ("); |
578 } |
606 for (dev_idx = EC_DEVICE_MAIN; |
|
607 dev_idx < ec_master_num_devices(domain->master); |
|
608 dev_idx++) { |
|
609 printk("%u", domain->working_counter[dev_idx]); |
|
610 if (dev_idx + 1 < ec_master_num_devices(domain->master)) { |
|
611 printk("+"); |
|
612 } |
|
613 } |
|
614 printk(")"); |
|
615 } |
|
616 printk(".\n"); |
|
617 |
579 domain->working_counter_changes = 0; |
618 domain->working_counter_changes = 0; |
580 } |
619 } |
581 } |
620 } |
582 |
621 |
583 /*****************************************************************************/ |
622 /*****************************************************************************/ |
587 ec_datagram_pair_t *datagram_pair; |
626 ec_datagram_pair_t *datagram_pair; |
588 ec_device_index_t dev_idx; |
627 ec_device_index_t dev_idx; |
589 |
628 |
590 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
629 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
591 |
630 |
|
631 #if EC_MAX_NUM_DEVICES > 1 |
592 /* copy main data to send buffer */ |
632 /* copy main data to send buffer */ |
593 memcpy(datagram_pair->send_buffer, |
633 memcpy(datagram_pair->send_buffer, |
594 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
634 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
595 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
635 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
|
636 #endif |
|
637 ec_master_queue_datagram(domain->master, |
|
638 &datagram_pair->datagrams[EC_DEVICE_MAIN]); |
596 |
639 |
597 /* copy main data to backup datagram */ |
640 /* copy main data to backup datagram */ |
598 memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, |
641 for (dev_idx = EC_DEVICE_BACKUP; |
599 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
642 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
600 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
643 memcpy(datagram_pair->datagrams[dev_idx].data, |
601 |
644 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
602 for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
645 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
603 ec_master_queue_datagram(domain->master, |
646 ec_master_queue_datagram(domain->master, |
604 &datagram_pair->datagrams[dev_idx]); |
647 &datagram_pair->datagrams[dev_idx]); |
605 } |
648 } |
606 } |
649 } |
607 } |
650 } |
608 |
651 |
609 /*****************************************************************************/ |
652 /*****************************************************************************/ |
610 |
653 |
611 void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state) |
654 void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state) |
612 { |
655 { |
613 state->working_counter = |
656 unsigned int dev_idx; |
614 domain->working_counter[EC_DEVICE_MAIN] |
657 uint16_t wc = 0; |
615 + domain->working_counter[EC_DEVICE_BACKUP]; |
658 |
|
659 for (dev_idx = EC_DEVICE_MAIN; |
|
660 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
661 wc += domain->working_counter[dev_idx]; |
|
662 } |
|
663 |
|
664 state->working_counter = wc; |
616 |
665 |
617 if (state->working_counter) { |
666 if (state->working_counter) { |
618 if (state->working_counter == domain->expected_working_counter) { |
667 if (state->working_counter == domain->expected_working_counter) { |
619 state->wc_state = EC_WC_COMPLETE; |
668 state->wc_state = EC_WC_COMPLETE; |
620 } else { |
669 } else { |