450 /*****************************************************************************/ |
450 /*****************************************************************************/ |
451 |
451 |
452 void ecrt_domain_process(ec_domain_t *domain) |
452 void ecrt_domain_process(ec_domain_t *domain) |
453 { |
453 { |
454 uint16_t wc_sum[EC_MAX_NUM_DEVICES] = {}, redundant_wc, wc_total; |
454 uint16_t wc_sum[EC_MAX_NUM_DEVICES] = {}, redundant_wc, wc_total; |
455 ec_datagram_pair_t *pair; |
|
456 ec_datagram_t *main_datagram; |
|
457 uint32_t logical_datagram_address; |
|
458 size_t datagram_size; |
|
459 uint16_t datagram_pair_wc; |
|
460 unsigned int dev_idx, wc_change; |
455 unsigned int dev_idx, wc_change; |
461 #if EC_MAX_NUM_DEVICES > 1 |
|
462 ec_fmmu_config_t *fmmu = |
|
463 list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list); |
|
464 #endif |
|
465 unsigned int redundancy; |
456 unsigned int redundancy; |
466 |
457 |
467 #if DEBUG_REDUNDANCY |
458 #if DEBUG_REDUNDANCY |
468 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
459 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
469 #endif |
460 #endif |
470 |
461 |
471 list_for_each_entry(pair, &domain->datagram_pairs, list) { |
462 #if EC_MAX_NUM_DEVICES > 1 |
472 |
463 if (ec_master_num_devices(domain->master) > 1) { |
473 main_datagram = &pair->datagrams[EC_DEVICE_MAIN]; |
464 ec_datagram_pair_t *pair; |
474 logical_datagram_address = EC_READ_U32(main_datagram->address); |
465 ec_fmmu_config_t *fmmu = |
475 datagram_size = main_datagram->data_size; |
466 list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list); |
|
467 |
|
468 list_for_each_entry(pair, &domain->datagram_pairs, list) { |
|
469 ec_datagram_t *main_datagram = &pair->datagrams[EC_DEVICE_MAIN]; |
|
470 uint32_t logical_datagram_address = |
|
471 EC_READ_U32(main_datagram->address); |
|
472 size_t datagram_size = main_datagram->data_size; |
|
473 uint16_t datagram_pair_wc; |
476 |
474 |
477 #if DEBUG_REDUNDANCY |
475 #if DEBUG_REDUNDANCY |
478 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
476 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
479 main_datagram->name, logical_datagram_address); |
477 main_datagram->name, logical_datagram_address); |
480 #endif |
478 #endif |
481 |
479 |
482 datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum); |
480 /* Redundancy: Go through FMMU configs to detect data changes. */ |
483 |
481 list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) { |
484 #if EC_MAX_NUM_DEVICES > 1 |
482 unsigned int datagram_offset; |
485 if (ec_master_num_devices(domain->master) < 2) { |
483 ec_datagram_t *backup_datagram = |
486 continue; |
484 &pair->datagrams[EC_DEVICE_BACKUP]; |
487 } |
485 |
488 |
486 if (fmmu->dir != EC_DIR_INPUT) { |
489 /* Redundancy: Go through all FMMU configs to detect data changes. */ |
487 continue; |
490 list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) { |
488 } |
491 unsigned int datagram_offset; |
489 |
492 ec_datagram_t *backup_datagram = |
490 if (fmmu->logical_start_address >= |
493 &pair->datagrams[EC_DEVICE_BACKUP]; |
491 logical_datagram_address + datagram_size) { |
494 |
492 // fmmu data contained in next datagram pair |
495 if (fmmu->dir != EC_DIR_INPUT) { |
493 break; |
496 continue; |
494 } |
497 } |
495 |
498 |
496 datagram_offset = |
499 if (fmmu->logical_start_address >= |
497 fmmu->logical_start_address - logical_datagram_address; |
500 logical_datagram_address + datagram_size) { |
498 |
501 // fmmu data contained in next datagram pair |
|
502 break; |
|
503 } |
|
504 |
|
505 datagram_offset = |
|
506 fmmu->logical_start_address - logical_datagram_address; |
|
507 |
|
508 #if DEBUG_REDUNDANCY |
|
509 EC_MASTER_DBG(domain->master, 1, |
|
510 "input fmmu log=%u size=%u offset=%u\n", |
|
511 fmmu->logical_start_address, fmmu->data_size, |
|
512 datagram_offset); |
|
513 if (domain->master->debug_level > 0) { |
|
514 ec_print_data(pair->send_buffer + datagram_offset, |
|
515 fmmu->data_size); |
|
516 ec_print_data(main_datagram->data + datagram_offset, |
|
517 fmmu->data_size); |
|
518 ec_print_data(backup_datagram->data + datagram_offset, |
|
519 fmmu->data_size); |
|
520 } |
|
521 #endif |
|
522 |
|
523 if (data_changed(pair->send_buffer, main_datagram, |
|
524 datagram_offset, fmmu->data_size)) { |
|
525 /* data changed on main link: no copying necessary. */ |
|
526 #if DEBUG_REDUNDANCY |
|
527 EC_MASTER_DBG(domain->master, 1, "main changed\n"); |
|
528 #endif |
|
529 } else if (data_changed(pair->send_buffer, backup_datagram, |
|
530 datagram_offset, fmmu->data_size)) { |
|
531 /* data changed on backup link: copy to main memory. */ |
|
532 #if DEBUG_REDUNDANCY |
|
533 EC_MASTER_DBG(domain->master, 1, "backup changed\n"); |
|
534 #endif |
|
535 memcpy(main_datagram->data + datagram_offset, |
|
536 backup_datagram->data + datagram_offset, |
|
537 fmmu->data_size); |
|
538 } else if (datagram_pair_wc == pair->expected_working_counter) { |
|
539 /* no change, but WC complete: use main data. */ |
|
540 #if DEBUG_REDUNDANCY |
|
541 EC_MASTER_DBG(domain->master, 1, "no change but complete\n"); |
|
542 #endif |
|
543 } else { |
|
544 /* no change and WC incomplete: mark WC as zero to avoid |
|
545 * data.dependent WC flickering. */ |
|
546 datagram_pair_wc = 0; |
|
547 #if DEBUG_REDUNDANCY |
499 #if DEBUG_REDUNDANCY |
548 EC_MASTER_DBG(domain->master, 1, |
500 EC_MASTER_DBG(domain->master, 1, |
549 "no change and incomplete\n"); |
501 "input fmmu log=%u size=%u offset=%u\n", |
550 #endif |
502 fmmu->logical_start_address, fmmu->data_size, |
|
503 datagram_offset); |
|
504 if (domain->master->debug_level > 0) { |
|
505 ec_print_data(pair->send_buffer + datagram_offset, |
|
506 fmmu->data_size); |
|
507 ec_print_data(main_datagram->data + datagram_offset, |
|
508 fmmu->data_size); |
|
509 ec_print_data(backup_datagram->data + datagram_offset, |
|
510 fmmu->data_size); |
|
511 } |
|
512 #endif |
|
513 datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum); |
|
514 |
|
515 if (data_changed(pair->send_buffer, main_datagram, |
|
516 datagram_offset, fmmu->data_size)) { |
|
517 /* data changed on main link: no copying necessary. */ |
|
518 #if DEBUG_REDUNDANCY |
|
519 EC_MASTER_DBG(domain->master, 1, "main changed\n"); |
|
520 #endif |
|
521 } else if (data_changed(pair->send_buffer, backup_datagram, |
|
522 datagram_offset, fmmu->data_size)) { |
|
523 /* data changed on backup link: copy to main memory. */ |
|
524 #if DEBUG_REDUNDANCY |
|
525 EC_MASTER_DBG(domain->master, 1, "backup changed\n"); |
|
526 #endif |
|
527 memcpy(main_datagram->data + datagram_offset, |
|
528 backup_datagram->data + datagram_offset, |
|
529 fmmu->data_size); |
|
530 } else if (datagram_pair_wc == |
|
531 pair->expected_working_counter) { |
|
532 /* no change, but WC complete: use main data. */ |
|
533 #if DEBUG_REDUNDANCY |
|
534 EC_MASTER_DBG(domain->master, 1, |
|
535 "no change but complete\n"); |
|
536 #endif |
|
537 } else { |
|
538 /* no change and WC incomplete: mark WC as zero to avoid |
|
539 * data.dependent WC flickering. */ |
|
540 datagram_pair_wc = 0; |
|
541 #if DEBUG_REDUNDANCY |
|
542 EC_MASTER_DBG(domain->master, 1, |
|
543 "no change and incomplete\n"); |
|
544 #endif |
|
545 } |
551 } |
546 } |
552 } |
547 } |
|
548 } |
553 #endif // EC_MAX_NUM_DEVICES > 1 |
549 #endif // EC_MAX_NUM_DEVICES > 1 |
554 } |
|
555 |
550 |
556 redundant_wc = 0; |
551 redundant_wc = 0; |
557 for (dev_idx = EC_DEVICE_BACKUP; |
552 for (dev_idx = EC_DEVICE_BACKUP; |
558 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
553 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
559 redundant_wc += wc_sum[dev_idx]; |
554 redundant_wc += wc_sum[dev_idx]; |