Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/domain.c
branchredundancy
changeset 2368 dd84ef164869
parent 2367 8527429b6137
child 2374 e898451c054a
equal deleted inserted replaced
2367:8527429b6137 2368:dd84ef164869 
    65     domain->data_size = 0;
 
    65     domain->data_size = 0;
 
    66     domain->data = NULL;
 
    66     domain->data = NULL;
 
    67     domain->data_origin = EC_ORIG_INTERNAL;
 
    67     domain->data_origin = EC_ORIG_INTERNAL;
 
    68     domain->logical_base_address = 0x00000000;
 
    68     domain->logical_base_address = 0x00000000;
 
    69     INIT_LIST_HEAD(&domain->datagram_pairs);
 
    69     INIT_LIST_HEAD(&domain->datagram_pairs);
 
    70     domain->working_counter = 0x0000;
 
    70     domain->working_counter[EC_DEVICE_MAIN] = 0x0000;
 
       
    71     domain->working_counter[EC_DEVICE_BACKUP] = 0x0000;
 
    71     domain->expected_working_counter = 0x0000;
 
    72     domain->expected_working_counter = 0x0000;
 
    72     domain->working_counter_changes = 0;
 
    73     domain->working_counter_changes = 0;
 
       
    74     domain->redundancy_active = 0;
 
    73     domain->notify_jiffies = 0;
 
    75     domain->notify_jiffies = 0;
 
    74 }
 
    76 }
 
    75 
    77 
    76 /*****************************************************************************/
 
    78 /*****************************************************************************/
 
    77 
    79 
   349     }
 
   351     }
 
   350 
   352 
   351     return NULL;
 
   353     return NULL;
 
   352 }
 
   354 }
 
   353 
   355 
       
   356 /*****************************************************************************/
 
       
   357 
       
   358 /** Process received data.
 
       
   359  */
 
       
   360 int data_changed(
 
       
   361         uint8_t *send_buffer,
 
       
   362         const ec_datagram_t *datagram,
 
       
   363         size_t offset,
 
       
   364         size_t size
 
       
   365         )
 
       
   366 {
 
       
   367     uint8_t *sent = send_buffer + offset;
 
       
   368     uint8_t *recv = datagram->data + offset;
 
       
   369     size_t i;
 
       
   370 
       
   371     for (i = 0; i < size; i++) {
 
       
   372         if (recv[i] != sent[i]) {
 
       
   373             return 1;
 
       
   374         }
 
       
   375     }
 
       
   376 
       
   377     return 0;
 
       
   378 }
 
       
   379 
   354 /******************************************************************************
 
   380 /******************************************************************************
 
   355  *  Application interface
 
   381  *  Application interface
 
   356  *****************************************************************************/
 
   382  *****************************************************************************/
 
   357 
   383 
   358 int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,
 
   384 int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,
 
   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 
   557 
   608 
   558 /*****************************************************************************/
 
   609 /*****************************************************************************/
 
   559 
   610 
   560 void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)
 
   611 void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)
 
   561 {
 
   612 {
 
   562     state->working_counter = domain->working_counter;
 
   613     state->working_counter =
 
   563 
   614         domain->working_counter[EC_DEVICE_MAIN]
 
   564     if (domain->working_counter) {
 
   615         + domain->working_counter[EC_DEVICE_BACKUP];
 
   565         if (domain->working_counter == domain->expected_working_counter) {
 
   616 
       
   617     if (state->working_counter) {
 
       
   618         if (state->working_counter == domain->expected_working_counter) {
 
   566             state->wc_state = EC_WC_COMPLETE;
 
   619             state->wc_state = EC_WC_COMPLETE;
 
   567         } else {
 
   620         } else {
 
   568             state->wc_state = EC_WC_INCOMPLETE;
 
   621             state->wc_state = EC_WC_INCOMPLETE;
 
   569         }
 
   622         }
 
   570     } else {
 
   623     } else {
 
   571         state->wc_state = EC_WC_ZERO;
 
   624         state->wc_state = EC_WC_ZERO;
 
   572     }
 
   625     }
 
       
   626 
       
   627     state->redundancy_active = domain->redundancy_active;
 
   573 }
 
   628 }
 
   574 
   629 
   575 /*****************************************************************************/
 
   630 /*****************************************************************************/
 
   576 
   631 
   577 /** \cond */
 
   632 /** \cond */
etherlabmaster