415 |
441 |
416 /*****************************************************************************/ |
442 /*****************************************************************************/ |
417 |
443 |
418 void ecrt_domain_process(ec_domain_t *domain) |
444 void ecrt_domain_process(ec_domain_t *domain) |
419 { |
445 { |
420 uint16_t working_counter_sum; |
446 uint16_t wc_sum[EC_NUM_DEVICES] = {}; |
421 ec_datagram_pair_t *datagram_pair = NULL; |
447 ec_datagram_pair_t *pair; |
422 ec_fmmu_config_t *fmmu; |
448 ec_datagram_t *main_datagram, *backup_datagram; |
423 uint32_t logical_datagram_address; |
449 uint32_t logical_datagram_address; |
424 unsigned int datagram_offset, datagram_pair_wc = 0; |
|
425 size_t datagram_size; |
450 size_t datagram_size; |
426 ec_datagram_t *main_datagram; |
451 uint16_t datagram_pair_wc; |
|
452 unsigned int datagram_offset; |
|
453 ec_fmmu_config_t *fmmu = |
|
454 list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list); |
|
455 unsigned int redundancy; |
427 |
456 |
428 #if DEBUG_REDUNDANCY |
457 #if DEBUG_REDUNDANCY |
429 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
458 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
430 #endif |
459 #endif |
431 |
460 |
432 working_counter_sum = 0; |
461 list_for_each_entry(pair, &domain->datagram_pairs, list) { |
433 |
462 |
434 if (!list_empty(&domain->datagram_pairs)) { |
463 main_datagram = &pair->datagrams[EC_DEVICE_MAIN]; |
435 datagram_pair = |
464 backup_datagram = &pair->datagrams[EC_DEVICE_BACKUP]; |
436 list_entry(domain->datagram_pairs.next, ec_datagram_pair_t, list); |
|
437 main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN]; |
|
438 |
|
439 logical_datagram_address = EC_READ_U32(main_datagram->address); |
465 logical_datagram_address = EC_READ_U32(main_datagram->address); |
440 datagram_size = main_datagram->data_size; |
466 datagram_size = main_datagram->data_size; |
441 datagram_offset = |
467 |
442 fmmu->logical_start_address - logical_datagram_address; |
|
443 datagram_pair_wc = ec_datagram_pair_process(datagram_pair); |
|
444 working_counter_sum += datagram_pair_wc; |
|
445 #if DEBUG_REDUNDANCY |
468 #if DEBUG_REDUNDANCY |
446 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
469 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
447 main_datagram->name, logical_datagram_address); |
470 main_datagram->name, logical_datagram_address); |
448 #endif |
471 #endif |
449 } |
472 |
450 |
473 datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum); |
451 /* Go through all FMMU configs to detect data changes. */ |
474 |
452 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
475 /* Go through all FMMU configs to detect data changes. */ |
453 #if DEBUG_REDUNDANCY |
476 list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) { |
454 EC_MASTER_DBG(domain->master, 1, "fmmu log=%u size=%u dir=%u\n", |
477 |
455 fmmu->logical_start_address, fmmu->data_size, fmmu->dir); |
478 if (fmmu->dir != EC_DIR_INPUT) { |
456 #endif |
479 continue; |
457 if (fmmu->dir != EC_DIR_INPUT) { |
480 } |
458 continue; |
481 |
459 } |
482 if (fmmu->logical_start_address >= |
460 |
483 logical_datagram_address + datagram_size) { |
461 logical_datagram_address = |
484 // fmmu data contained in next datagram pair |
462 EC_READ_U32(datagram_pair->datagrams[EC_DEVICE_MAIN].address); |
485 break; |
463 datagram_size = datagram_pair->datagrams[EC_DEVICE_MAIN].data_size; |
486 } |
464 datagram_offset = |
487 |
465 fmmu->logical_start_address - logical_datagram_address; |
|
466 while (datagram_offset >= datagram_size) { |
|
467 |
|
468 datagram_pair = list_entry(datagram_pair->list.next, |
|
469 ec_datagram_pair_t, list); |
|
470 main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN]; |
|
471 |
|
472 logical_datagram_address = EC_READ_U32(main_datagram->address); |
|
473 datagram_size = main_datagram->data_size; |
|
474 datagram_offset = |
488 datagram_offset = |
475 fmmu->logical_start_address - logical_datagram_address; |
489 fmmu->logical_start_address - logical_datagram_address; |
476 datagram_pair_wc = ec_datagram_pair_process(datagram_pair); |
490 |
477 working_counter_sum += datagram_pair_wc; |
|
478 #if DEBUG_REDUNDANCY |
491 #if DEBUG_REDUNDANCY |
479 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
492 EC_MASTER_DBG(domain->master, 1, |
480 main_datagram->name, logical_datagram_address); |
493 "input fmmu log=%u size=%u offset=%u\n", |
|
494 fmmu->logical_start_address, fmmu->data_size, |
|
495 datagram_offset); |
|
496 if (domain->master->debug_level > 0) { |
|
497 ec_print_data(pair->send_buffer + datagram_offset, |
|
498 fmmu->data_size); |
|
499 ec_print_data(main_datagram->data + datagram_offset, |
|
500 fmmu->data_size); |
|
501 ec_print_data(backup_datagram->data + datagram_offset, |
|
502 fmmu->data_size); |
|
503 } |
481 #endif |
504 #endif |
482 } |
505 |
483 |
506 if (data_changed(pair->send_buffer, main_datagram, |
|
507 datagram_offset, fmmu->data_size)) { |
|
508 /* data changed on main link: no copying necessary. */ |
484 #if DEBUG_REDUNDANCY |
509 #if DEBUG_REDUNDANCY |
485 EC_MASTER_DBG(domain->master, 1, "input fmmu log=%u size=%u" |
510 EC_MASTER_DBG(domain->master, 1, "main changed\n"); |
486 " using datagram %s offset=%u\n", |
|
487 fmmu->logical_start_address, fmmu->data_size, |
|
488 datagram_pair->datagrams[EC_DEVICE_MAIN].name, |
|
489 datagram_offset); |
|
490 #endif |
511 #endif |
491 |
512 } else if (data_changed(pair->send_buffer, backup_datagram, |
492 if (ec_datagram_pair_data_changed(datagram_pair, |
513 datagram_offset, fmmu->data_size)) { |
493 datagram_offset, fmmu->data_size, EC_DEVICE_MAIN)) { |
514 /* data changed on backup link: copy to main memory. */ |
494 /* data changed on main link. no copying necessary. */ |
515 #if DEBUG_REDUNDANCY |
495 } else if (ec_datagram_pair_data_changed(datagram_pair, |
516 EC_MASTER_DBG(domain->master, 1, "backup changed\n"); |
496 datagram_offset, fmmu->data_size, EC_DEVICE_BACKUP) |
517 #endif |
497 || (datagram_pair_wc |
518 memcpy(main_datagram->data + datagram_offset, |
498 == datagram_pair->expected_working_counter)) { |
519 backup_datagram->data + datagram_offset, |
499 /* data changed on backup link or no change and complete WC. |
520 fmmu->data_size); |
500 * copy to main memory. */ |
521 } else if (datagram_pair_wc == pair->expected_working_counter) { |
501 uint8_t *target = datagram_pair->datagrams[EC_DEVICE_MAIN].data + |
522 /* no change, but WC complete: use main data. */ |
502 datagram_offset; |
523 #if DEBUG_REDUNDANCY |
503 uint8_t *source = datagram_pair->datagrams[EC_DEVICE_BACKUP].data + |
524 EC_MASTER_DBG(domain->master, 1, "no change but complete\n"); |
504 datagram_offset; |
525 #endif |
505 memcpy(target, source, fmmu->data_size); |
526 } else { |
506 } |
527 /* no change and WC incomplete: mark WC as zero to avoid |
507 } |
528 * data.dependent WC flickering. */ |
508 |
529 datagram_pair_wc = 0; |
509 if (working_counter_sum != domain->working_counter) { |
530 #if DEBUG_REDUNDANCY |
|
531 EC_MASTER_DBG(domain->master, 1, |
|
532 "no change and incomplete\n"); |
|
533 #endif |
|
534 } |
|
535 } |
|
536 } |
|
537 |
|
538 redundancy = wc_sum[EC_DEVICE_BACKUP] > 0; |
|
539 if (redundancy != domain->redundancy_active) { |
|
540 if (redundancy) { |
|
541 EC_MASTER_WARN(domain->master, |
|
542 "Domain %u: Redundant link in use!\n", |
|
543 domain->index); |
|
544 } else { |
|
545 EC_MASTER_INFO(domain->master, |
|
546 "Domain %u: Redundant link unused again.\n", |
|
547 domain->index); |
|
548 } |
|
549 domain->redundancy_active = redundancy; |
|
550 } |
|
551 |
|
552 if ((wc_sum[EC_DEVICE_MAIN] != domain->working_counter[EC_DEVICE_MAIN]) |
|
553 || (wc_sum[EC_DEVICE_BACKUP] |
|
554 != domain->working_counter[EC_DEVICE_BACKUP])) { |
510 domain->working_counter_changes++; |
555 domain->working_counter_changes++; |
511 domain->working_counter = working_counter_sum; |
556 domain->working_counter[EC_DEVICE_MAIN] = wc_sum[EC_DEVICE_MAIN]; |
|
557 domain->working_counter[EC_DEVICE_BACKUP] = wc_sum[EC_DEVICE_BACKUP]; |
512 } |
558 } |
513 |
559 |
514 if (domain->working_counter_changes && |
560 if (domain->working_counter_changes && |
515 jiffies - domain->notify_jiffies > HZ) { |
561 jiffies - domain->notify_jiffies > HZ) { |
516 domain->notify_jiffies = jiffies; |
562 domain->notify_jiffies = jiffies; |
517 if (domain->working_counter_changes == 1) { |
563 if (domain->working_counter_changes == 1) { |
518 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
564 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
519 " changed to %u/%u.\n", domain->index, |
565 " changed to %u/%u (%u+%u).\n", domain->index, |
520 domain->working_counter, |
566 domain->working_counter[EC_DEVICE_MAIN] + |
521 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]); |
522 } else { |
570 } else { |
523 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
571 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
524 " changes - now %u/%u.\n", domain->index, |
572 " changes - now %u/%u (%u+%u).\n", domain->index, |
525 domain->working_counter_changes, domain->working_counter, |
573 domain->working_counter_changes, |
526 domain->expected_working_counter); |
574 domain->working_counter[EC_DEVICE_MAIN] + |
|
575 domain->working_counter[EC_DEVICE_BACKUP], |
|
576 domain->expected_working_counter, |
|
577 wc_sum[EC_DEVICE_MAIN], wc_sum[EC_DEVICE_BACKUP]); |
527 } |
578 } |
528 domain->working_counter_changes = 0; |
579 domain->working_counter_changes = 0; |
529 } |
580 } |
530 } |
581 } |
531 |
582 |