142 const unsigned int used[] /**< Slave config counter for in/out. */ |
144 const unsigned int used[] /**< Slave config counter for in/out. */ |
143 ) |
145 ) |
144 { |
146 { |
145 ec_datagram_pair_t *datagram_pair; |
147 ec_datagram_pair_t *datagram_pair; |
146 int ret; |
148 int ret; |
147 unsigned int dev_idx; |
|
148 |
149 |
149 if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { |
150 if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { |
150 EC_MASTER_ERR(domain->master, |
151 EC_MASTER_ERR(domain->master, |
151 "Failed to allocate domain datagram pair!\n"); |
152 "Failed to allocate domain datagram pair!\n"); |
152 return -ENOMEM; |
153 return -ENOMEM; |
153 } |
154 } |
154 |
155 |
155 ec_datagram_pair_init(datagram_pair); |
156 ret = ec_datagram_pair_init(datagram_pair, domain, logical_offset, data, |
156 |
157 data_size, used); |
157 /* backup datagram has its own memory */ |
|
158 ret = ec_datagram_prealloc(&datagram_pair->datagrams[EC_DEVICE_BACKUP], |
|
159 data_size); |
|
160 if (ret) { |
158 if (ret) { |
161 ec_datagram_pair_clear(datagram_pair); |
|
162 kfree(datagram_pair); |
159 kfree(datagram_pair); |
163 return ret; |
160 return ret; |
164 } |
161 } |
165 |
162 |
166 /* The ec_datagram_lxx() calls below can not fail, because either the |
163 domain->expected_working_counter += |
167 * datagram has external memory or it is preallocated. */ |
164 datagram_pair->expected_working_counter; |
168 |
165 |
169 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
166 EC_MASTER_DBG(domain->master, 1, |
170 ec_datagram_lrw_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN], |
167 "Adding datagram pair with expected WC %u.\n", |
171 logical_offset, data_size, data); |
168 datagram_pair->expected_working_counter); |
172 ec_datagram_lrw(&datagram_pair->datagrams[EC_DEVICE_BACKUP], |
169 |
173 logical_offset, data_size); |
|
174 |
|
175 // If LRW is used, output FMMUs increment the working counter by 2, |
|
176 // while input FMMUs increment it by 1. |
|
177 domain->expected_working_counter += |
|
178 used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; |
|
179 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
|
180 ec_datagram_lwr_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN], |
|
181 logical_offset, data_size, data); |
|
182 ec_datagram_lwr(&datagram_pair->datagrams[EC_DEVICE_BACKUP], |
|
183 logical_offset, data_size); |
|
184 |
|
185 domain->expected_working_counter += used[EC_DIR_OUTPUT]; |
|
186 } else { // inputs only (or nothing) |
|
187 ec_datagram_lrd_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN], |
|
188 logical_offset, data_size, data); |
|
189 ec_datagram_lrd(&datagram_pair->datagrams[EC_DEVICE_BACKUP], |
|
190 logical_offset, data_size); |
|
191 |
|
192 domain->expected_working_counter += used[EC_DIR_INPUT]; |
|
193 } |
|
194 |
|
195 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
196 snprintf(datagram_pair->datagrams[dev_idx].name, |
|
197 EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index, |
|
198 logical_offset, dev_idx ? "backup" : "main"); |
|
199 ec_datagram_zero(&datagram_pair->datagrams[dev_idx]); |
|
200 } |
|
201 |
170 |
202 list_add_tail(&datagram_pair->list, &domain->datagram_pairs); |
171 list_add_tail(&datagram_pair->list, &domain->datagram_pairs); |
203 return 0; |
172 return 0; |
204 } |
173 } |
205 |
174 |
447 /*****************************************************************************/ |
416 /*****************************************************************************/ |
448 |
417 |
449 void ecrt_domain_process(ec_domain_t *domain) |
418 void ecrt_domain_process(ec_domain_t *domain) |
450 { |
419 { |
451 uint16_t working_counter_sum; |
420 uint16_t working_counter_sum; |
452 ec_datagram_pair_t *datagram_pair; |
421 ec_datagram_pair_t *datagram_pair = NULL; |
453 unsigned int dev_idx; |
422 ec_fmmu_config_t *fmmu; |
|
423 uint32_t logical_datagram_address; |
|
424 unsigned int datagram_offset, datagram_pair_wc = 0; |
|
425 size_t datagram_size; |
|
426 ec_datagram_t *main_datagram; |
|
427 |
|
428 #if DEBUG_REDUNDANCY |
|
429 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
|
430 #endif |
454 |
431 |
455 working_counter_sum = 0; |
432 working_counter_sum = 0; |
456 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
433 |
457 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
434 if (!list_empty(&domain->datagram_pairs)) { |
458 ec_datagram_t *datagram = &datagram_pair->datagrams[dev_idx]; |
435 datagram_pair = |
459 ec_datagram_output_stats(datagram); |
436 list_entry(domain->datagram_pairs.next, ec_datagram_pair_t, list); |
460 if (datagram->state == EC_DATAGRAM_RECEIVED) { |
437 main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN]; |
461 working_counter_sum += datagram->working_counter; |
438 |
462 } |
439 logical_datagram_address = EC_READ_U32(main_datagram->address); |
|
440 datagram_size = main_datagram->data_size; |
|
441 datagram_offset = |
|
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 |
|
446 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
|
447 main_datagram->name, logical_datagram_address); |
|
448 #endif |
|
449 } |
|
450 |
|
451 /* Go through all FMMU configs to detect data changes. */ |
|
452 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
|
453 #if DEBUG_REDUNDANCY |
|
454 EC_MASTER_DBG(domain->master, 1, "fmmu log=%u size=%u dir=%u\n", |
|
455 fmmu->logical_start_address, fmmu->data_size, fmmu->dir); |
|
456 #endif |
|
457 if (fmmu->dir != EC_DIR_INPUT) { |
|
458 continue; |
|
459 } |
|
460 |
|
461 logical_datagram_address = |
|
462 EC_READ_U32(datagram_pair->datagrams[EC_DEVICE_MAIN].address); |
|
463 datagram_size = datagram_pair->datagrams[EC_DEVICE_MAIN].data_size; |
|
464 datagram_offset = |
|
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 = |
|
475 fmmu->logical_start_address - logical_datagram_address; |
|
476 datagram_pair_wc = ec_datagram_pair_process(datagram_pair); |
|
477 working_counter_sum += datagram_pair_wc; |
|
478 #if DEBUG_REDUNDANCY |
|
479 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
|
480 main_datagram->name, logical_datagram_address); |
|
481 #endif |
|
482 } |
|
483 |
|
484 #if DEBUG_REDUNDANCY |
|
485 EC_MASTER_DBG(domain->master, 1, "input fmmu log=%u size=%u" |
|
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 |
|
491 |
|
492 if (ec_datagram_pair_data_changed(datagram_pair, |
|
493 datagram_offset, fmmu->data_size, EC_DEVICE_MAIN)) { |
|
494 /* data changed on main link. no copying necessary. */ |
|
495 } else if (ec_datagram_pair_data_changed(datagram_pair, |
|
496 datagram_offset, fmmu->data_size, EC_DEVICE_BACKUP) |
|
497 || (datagram_pair_wc |
|
498 == datagram_pair->expected_working_counter)) { |
|
499 /* data changed on backup link or no change and complete WC. |
|
500 * copy to main memory. */ |
|
501 uint8_t *target = datagram_pair->datagrams[EC_DEVICE_MAIN].data + |
|
502 datagram_offset; |
|
503 uint8_t *source = datagram_pair->datagrams[EC_DEVICE_BACKUP].data + |
|
504 datagram_offset; |
|
505 memcpy(target, source, fmmu->data_size); |
463 } |
506 } |
464 } |
507 } |
465 |
508 |
466 if (working_counter_sum != domain->working_counter) { |
509 if (working_counter_sum != domain->working_counter) { |
467 domain->working_counter_changes++; |
510 domain->working_counter_changes++; |
493 ec_datagram_pair_t *datagram_pair; |
536 ec_datagram_pair_t *datagram_pair; |
494 unsigned int dev_idx; |
537 unsigned int dev_idx; |
495 |
538 |
496 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
539 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
497 |
540 |
|
541 /* copy main data to send buffer */ |
|
542 memcpy(datagram_pair->send_buffer, |
|
543 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
|
544 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
|
545 |
498 /* copy main data to backup datagram */ |
546 /* copy main data to backup datagram */ |
499 memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, |
547 memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, |
500 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
548 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
501 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
549 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
502 |
550 |