142 const unsigned int used[] /**< Used by inputs/outputs. */ |
142 const unsigned int used[] /**< Used by inputs/outputs. */ |
143 ) |
143 ) |
144 { |
144 { |
145 ec_datagram_pair_t *datagram_pair; |
145 ec_datagram_pair_t *datagram_pair; |
146 int ret; |
146 int ret; |
147 unsigned int i; |
147 unsigned int dev_idx; |
148 |
148 |
149 if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { |
149 if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { |
150 EC_MASTER_ERR(domain->master, |
150 EC_MASTER_ERR(domain->master, |
151 "Failed to allocate domain datagram pair!\n"); |
151 "Failed to allocate domain datagram pair!\n"); |
152 return -ENOMEM; |
152 return -ENOMEM; |
190 logical_offset, data_size); |
190 logical_offset, data_size); |
191 |
191 |
192 domain->expected_working_counter += used[EC_DIR_INPUT]; |
192 domain->expected_working_counter += used[EC_DIR_INPUT]; |
193 } |
193 } |
194 |
194 |
195 for (i = 0; i < EC_NUM_DEVICES; i++) { |
195 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
196 snprintf(datagram_pair->datagrams[i].name, EC_DATAGRAM_NAME_SIZE, |
196 snprintf(datagram_pair->datagrams[dev_idx].name, |
197 "domain%u-%u-%s", domain->index, logical_offset, |
197 EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index, |
198 i ? "backup" : "main"); |
198 logical_offset, dev_idx ? "backup" : "main"); |
199 ec_datagram_zero(&datagram_pair->datagrams[i]); |
199 ec_datagram_zero(&datagram_pair->datagrams[dev_idx]); |
200 } |
200 } |
201 |
201 |
202 list_add_tail(&datagram_pair->list, &domain->datagram_pairs); |
202 list_add_tail(&datagram_pair->list, &domain->datagram_pairs); |
203 return 0; |
203 return 0; |
204 } |
204 } |
422 |
422 |
423 void ecrt_domain_process(ec_domain_t *domain) |
423 void ecrt_domain_process(ec_domain_t *domain) |
424 { |
424 { |
425 uint16_t working_counter_sum; |
425 uint16_t working_counter_sum; |
426 ec_datagram_pair_t *datagram_pair; |
426 ec_datagram_pair_t *datagram_pair; |
427 unsigned int i; |
427 unsigned int dev_idx; |
428 |
428 |
429 working_counter_sum = 0; |
429 working_counter_sum = 0; |
430 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
430 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
431 for (i = 0; i < EC_NUM_DEVICES; i++) { |
431 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
432 ec_datagram_t *datagram = &datagram_pair->datagrams[i]; |
432 ec_datagram_t *datagram = &datagram_pair->datagrams[dev_idx]; |
433 ec_datagram_output_stats(datagram); |
433 ec_datagram_output_stats(datagram); |
434 if (datagram->state == EC_DATAGRAM_RECEIVED) { |
434 if (datagram->state == EC_DATAGRAM_RECEIVED) { |
435 working_counter_sum += datagram->working_counter; |
435 working_counter_sum += datagram->working_counter; |
436 } |
436 } |
437 } |
437 } |
446 jiffies - domain->notify_jiffies > HZ) { |
446 jiffies - domain->notify_jiffies > HZ) { |
447 domain->notify_jiffies = jiffies; |
447 domain->notify_jiffies = jiffies; |
448 if (domain->working_counter_changes == 1) { |
448 if (domain->working_counter_changes == 1) { |
449 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
449 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
450 " changed to %u/%u.\n", domain->index, |
450 " changed to %u/%u.\n", domain->index, |
451 domain->working_counter, domain->expected_working_counter); |
451 domain->working_counter, |
|
452 domain->expected_working_counter); |
452 } else { |
453 } else { |
453 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
454 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
454 " changes - now %u/%u.\n", domain->index, |
455 " changes - now %u/%u.\n", domain->index, |
455 domain->working_counter_changes, domain->working_counter, |
456 domain->working_counter_changes, domain->working_counter, |
456 domain->expected_working_counter); |
457 domain->expected_working_counter); |
462 /*****************************************************************************/ |
463 /*****************************************************************************/ |
463 |
464 |
464 void ecrt_domain_queue(ec_domain_t *domain) |
465 void ecrt_domain_queue(ec_domain_t *domain) |
465 { |
466 { |
466 ec_datagram_pair_t *datagram_pair; |
467 ec_datagram_pair_t *datagram_pair; |
467 unsigned int i; |
468 unsigned int dev_idx; |
468 |
469 |
469 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
470 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
470 |
471 |
471 /* copy main data to backup datagram */ |
472 /* copy main data to backup datagram */ |
472 memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, |
473 memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, |
473 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
474 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
474 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
475 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
475 |
476 |
476 for (i = 0; i < EC_NUM_DEVICES; i++) { |
477 for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
477 ec_master_queue_datagram(domain->master, |
478 ec_master_queue_datagram(domain->master, |
478 &datagram_pair->datagrams[i], i); |
479 &datagram_pair->datagrams[dev_idx], dev_idx); |
479 } |
480 } |
480 } |
481 } |
481 } |
482 } |
482 |
483 |
483 /*****************************************************************************/ |
484 /*****************************************************************************/ |