61 INIT_LIST_HEAD(&domain->fmmu_configs); |
64 INIT_LIST_HEAD(&domain->fmmu_configs); |
62 domain->data_size = 0; |
65 domain->data_size = 0; |
63 domain->data = NULL; |
66 domain->data = NULL; |
64 domain->data_origin = EC_ORIG_INTERNAL; |
67 domain->data_origin = EC_ORIG_INTERNAL; |
65 domain->logical_base_address = 0x00000000; |
68 domain->logical_base_address = 0x00000000; |
66 INIT_LIST_HEAD(&domain->datagrams); |
69 INIT_LIST_HEAD(&domain->datagram_pairs); |
67 domain->working_counter = 0x0000; |
70 domain->working_counter[EC_DEVICE_MAIN] = 0x0000; |
|
71 domain->working_counter[EC_DEVICE_BACKUP] = 0x0000; |
68 domain->expected_working_counter = 0x0000; |
72 domain->expected_working_counter = 0x0000; |
69 domain->working_counter_changes = 0; |
73 domain->working_counter_changes = 0; |
|
74 domain->redundancy_active = 0; |
70 domain->notify_jiffies = 0; |
75 domain->notify_jiffies = 0; |
71 } |
76 } |
72 |
77 |
73 /*****************************************************************************/ |
78 /*****************************************************************************/ |
74 |
79 |
75 /** Domain destructor. |
80 /** Domain destructor. |
76 */ |
81 */ |
77 void ec_domain_clear(ec_domain_t *domain /**< EtherCAT domain */) |
82 void ec_domain_clear(ec_domain_t *domain /**< EtherCAT domain */) |
78 { |
83 { |
79 ec_datagram_t *datagram, *next; |
84 ec_datagram_pair_t *datagram_pair, *next_pair; |
80 |
85 |
81 // dequeue and free datagrams |
86 // dequeue and free datagrams |
82 list_for_each_entry_safe(datagram, next, &domain->datagrams, list) { |
87 list_for_each_entry_safe(datagram_pair, next_pair, |
83 ec_datagram_clear(datagram); |
88 &domain->datagram_pairs, list) { |
84 kfree(datagram); |
89 ec_datagram_pair_clear(datagram_pair); |
|
90 kfree(datagram_pair); |
85 } |
91 } |
86 |
92 |
87 ec_domain_clear_data(domain); |
93 ec_domain_clear_data(domain); |
88 } |
94 } |
89 |
95 |
120 domain->index, fmmu->data_size, domain->data_size); |
128 domain->index, fmmu->data_size, domain->data_size); |
121 } |
129 } |
122 |
130 |
123 /*****************************************************************************/ |
131 /*****************************************************************************/ |
124 |
132 |
125 /** Allocates a domain datagram and appends it to the list. |
133 /** Allocates a domain datagram pair and appends it to the list. |
126 * |
134 * |
127 * The datagram type and expected working counters are determined by the |
135 * The datagrams' types and expected working counters are determined by the |
128 * number of input and output fmmus that share the datagram. |
136 * number of input and output fmmus that share the datagrams. |
129 * |
137 * |
130 * \retval 0 Success. |
138 * \retval 0 Success. |
131 * \retval <0 Error code. |
139 * \retval <0 Error code. |
132 */ |
140 */ |
133 int ec_domain_add_datagram( |
141 int ec_domain_add_datagram_pair( |
134 ec_domain_t *domain, /**< EtherCAT domain. */ |
142 ec_domain_t *domain, /**< EtherCAT domain. */ |
135 uint32_t logical_offset, /**< Logical offset. */ |
143 uint32_t logical_offset, /**< Logical offset. */ |
136 size_t data_size, /**< Size of the data. */ |
144 size_t data_size, /**< Size of the data. */ |
137 uint8_t *data, /**< Process data. */ |
145 uint8_t *data, /**< Process data. */ |
138 const unsigned int used[] /**< Used by inputs/outputs. */ |
146 const unsigned int used[] /**< Slave config counter for in/out. */ |
139 ) |
147 ) |
140 { |
148 { |
141 ec_datagram_t *datagram; |
149 ec_datagram_pair_t *datagram_pair; |
142 int ret; |
150 int ret; |
143 |
151 |
144 if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { |
152 if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) { |
145 EC_MASTER_ERR(domain->master, |
153 EC_MASTER_ERR(domain->master, |
146 "Failed to allocate domain datagram!\n"); |
154 "Failed to allocate domain datagram pair!\n"); |
147 return -ENOMEM; |
155 return -ENOMEM; |
148 } |
156 } |
149 |
157 |
150 ec_datagram_init(datagram); |
158 ret = ec_datagram_pair_init(datagram_pair, domain, logical_offset, data, |
151 snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, |
159 data_size, used); |
152 "domain%u-%u", domain->index, logical_offset); |
160 if (ret) { |
153 |
161 kfree(datagram_pair); |
154 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
162 return ret; |
155 ret = ec_datagram_lrw(datagram, logical_offset, data_size, data); |
163 } |
156 if (ret < 0) { |
164 |
157 kfree(datagram); |
165 domain->expected_working_counter += |
158 return ret; |
166 datagram_pair->expected_working_counter; |
159 } |
167 |
160 // If LRW is used, output FMMUs increment the working counter by 2, |
168 EC_MASTER_DBG(domain->master, 1, |
161 // while input FMMUs increment it by 1. |
169 "Adding datagram pair with expected WC %u.\n", |
162 domain->expected_working_counter += |
170 datagram_pair->expected_working_counter); |
163 used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; |
171 |
164 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
172 |
165 ret = ec_datagram_lwr(datagram, logical_offset, data_size, data); |
173 list_add_tail(&datagram_pair->list, &domain->datagram_pairs); |
166 if (ret < 0) { |
|
167 kfree(datagram); |
|
168 return ret; |
|
169 } |
|
170 domain->expected_working_counter += used[EC_DIR_OUTPUT]; |
|
171 } else { // inputs only (or nothing) |
|
172 ret = ec_datagram_lrd(datagram, logical_offset, data_size, data); |
|
173 if (ret < 0) { |
|
174 kfree(datagram); |
|
175 return ret; |
|
176 } |
|
177 domain->expected_working_counter += used[EC_DIR_INPUT]; |
|
178 } |
|
179 |
|
180 ec_datagram_zero(datagram); |
|
181 list_add_tail(&datagram->list, &domain->datagrams); |
|
182 return 0; |
174 return 0; |
|
175 } |
|
176 |
|
177 /*****************************************************************************/ |
|
178 |
|
179 /** Domain finish helper function. |
|
180 * |
|
181 * Detects, if a slave configuration has already been taken into account for |
|
182 * a datagram's expected working counter calculation. |
|
183 * |
|
184 * Walks through the list of all FMMU configurations for the current datagram |
|
185 * and ends before the current datagram. |
|
186 */ |
|
187 int shall_count( |
|
188 const ec_fmmu_config_t *cur_fmmu, /**< Current FMMU with direction to |
|
189 search for. */ |
|
190 const ec_fmmu_config_t *first_fmmu /**< Datagram's first FMMU. */ |
|
191 ) |
|
192 { |
|
193 for (; first_fmmu != cur_fmmu; |
|
194 first_fmmu = list_entry(first_fmmu->list.next, |
|
195 ec_fmmu_config_t, list)) { |
|
196 |
|
197 if (first_fmmu->sc == cur_fmmu->sc |
|
198 && first_fmmu->dir == cur_fmmu->dir) { |
|
199 return 0; // was already counted |
|
200 } |
|
201 } |
|
202 |
|
203 return 1; |
183 } |
204 } |
184 |
205 |
185 /*****************************************************************************/ |
206 /*****************************************************************************/ |
186 |
207 |
187 /** Finishes a domain. |
208 /** Finishes a domain. |
218 domain->data_size, domain->index); |
239 domain->data_size, domain->index); |
219 return -ENOMEM; |
240 return -ENOMEM; |
220 } |
241 } |
221 } |
242 } |
222 |
243 |
223 // Cycle through all domain FMMUS and |
244 // Cycle through all domain FMMUs and |
224 // - correct the logical base addresses |
245 // - correct the logical base addresses |
225 // - set up the datagrams to carry the process data |
246 // - set up the datagrams to carry the process data |
|
247 // - calculate the datagrams' expected working counters |
226 datagram_offset = 0; |
248 datagram_offset = 0; |
227 datagram_size = 0; |
249 datagram_size = 0; |
228 datagram_count = 0; |
250 datagram_count = 0; |
229 datagram_used[EC_DIR_OUTPUT] = 0; |
251 datagram_used[EC_DIR_OUTPUT] = 0; |
230 datagram_used[EC_DIR_INPUT] = 0; |
252 datagram_used[EC_DIR_INPUT] = 0; |
231 |
253 |
232 list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) { |
254 if (!list_empty(&domain->fmmu_configs)) { |
233 // we have to remove the constness, sorry FIXME |
255 datagram_first_fmmu = |
234 ec_slave_config_t *sc = (ec_slave_config_t *) fmmu_temp->sc; |
256 list_entry(domain->fmmu_configs.next, ec_fmmu_config_t, list); |
235 sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0; |
|
236 } |
257 } |
237 |
258 |
238 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
259 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
|
260 |
239 // Correct logical FMMU address |
261 // Correct logical FMMU address |
240 fmmu->logical_start_address += base_address; |
262 fmmu->logical_start_address += base_address; |
241 |
263 |
242 // Increment Input/Output counter to determine datagram types |
264 // Increment Input/Output counter to determine datagram types |
243 // and calculate expected working counters |
265 // and calculate expected working counters |
244 if (fmmu->sc->used_for_fmmu_datagram[fmmu->dir] == 0) { |
266 if (shall_count(fmmu, datagram_first_fmmu)) { |
245 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu->sc; |
|
246 datagram_used[fmmu->dir]++; |
267 datagram_used[fmmu->dir]++; |
247 sc->used_for_fmmu_datagram[fmmu->dir] = 1; |
|
248 } |
268 } |
249 |
269 |
250 // If the current FMMU's data do not fit in the current datagram, |
270 // If the current FMMU's data do not fit in the current datagram, |
251 // allocate a new one. |
271 // allocate a new one. |
252 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
272 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
253 ret = ec_domain_add_datagram(domain, |
273 ret = ec_domain_add_datagram_pair(domain, |
254 domain->logical_base_address + datagram_offset, |
274 domain->logical_base_address + datagram_offset, |
255 datagram_size, domain->data + datagram_offset, |
275 datagram_size, domain->data + datagram_offset, |
256 datagram_used); |
276 datagram_used); |
257 if (ret < 0) |
277 if (ret < 0) |
258 return ret; |
278 return ret; |
|
279 |
259 datagram_offset += datagram_size; |
280 datagram_offset += datagram_size; |
260 datagram_size = 0; |
281 datagram_size = 0; |
261 datagram_count++; |
282 datagram_count++; |
262 datagram_used[EC_DIR_OUTPUT] = 0; |
283 datagram_used[EC_DIR_OUTPUT] = 0; |
263 datagram_used[EC_DIR_INPUT] = 0; |
284 datagram_used[EC_DIR_INPUT] = 0; |
264 list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) { |
285 datagram_first_fmmu = fmmu; |
265 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc; |
|
266 sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0; |
|
267 } |
|
268 } |
286 } |
269 |
287 |
270 datagram_size += fmmu->data_size; |
288 datagram_size += fmmu->data_size; |
271 } |
289 } |
272 |
290 |
273 // Allocate last datagram, if data are left (this is also the case if the |
291 /* Allocate last datagram pair, if data are left (this is also the case if |
274 // process data fit into a single datagram) |
292 * the process data fit into a single datagram) */ |
275 if (datagram_size) { |
293 if (datagram_size) { |
276 ret = ec_domain_add_datagram(domain, |
294 ret = ec_domain_add_datagram_pair(domain, |
277 domain->logical_base_address + datagram_offset, |
295 domain->logical_base_address + datagram_offset, |
278 datagram_size, domain->data + datagram_offset, |
296 datagram_size, domain->data + datagram_offset, |
279 datagram_used); |
297 datagram_used); |
280 if (ret < 0) |
298 if (ret < 0) |
281 return ret; |
299 return ret; |
396 |
441 |
397 /*****************************************************************************/ |
442 /*****************************************************************************/ |
398 |
443 |
399 void ecrt_domain_process(ec_domain_t *domain) |
444 void ecrt_domain_process(ec_domain_t *domain) |
400 { |
445 { |
401 uint16_t working_counter_sum; |
446 uint16_t wc_sum[EC_NUM_DEVICES] = {}; |
402 ec_datagram_t *datagram; |
447 ec_datagram_pair_t *pair; |
403 |
448 ec_datagram_t *main_datagram, *backup_datagram; |
404 working_counter_sum = 0x0000; |
449 uint32_t logical_datagram_address; |
405 list_for_each_entry(datagram, &domain->datagrams, list) { |
450 size_t datagram_size; |
406 ec_datagram_output_stats(datagram); |
451 uint16_t datagram_pair_wc; |
407 if (datagram->state == EC_DATAGRAM_RECEIVED) { |
452 unsigned int datagram_offset; |
408 working_counter_sum += datagram->working_counter; |
453 ec_fmmu_config_t *fmmu = |
409 } |
454 list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list); |
410 } |
455 unsigned int redundancy; |
411 |
456 |
412 if (working_counter_sum != domain->working_counter) { |
457 #if DEBUG_REDUNDANCY |
|
458 EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index); |
|
459 #endif |
|
460 |
|
461 list_for_each_entry(pair, &domain->datagram_pairs, list) { |
|
462 |
|
463 main_datagram = &pair->datagrams[EC_DEVICE_MAIN]; |
|
464 backup_datagram = &pair->datagrams[EC_DEVICE_BACKUP]; |
|
465 logical_datagram_address = EC_READ_U32(main_datagram->address); |
|
466 datagram_size = main_datagram->data_size; |
|
467 |
|
468 #if DEBUG_REDUNDANCY |
|
469 EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n", |
|
470 main_datagram->name, logical_datagram_address); |
|
471 #endif |
|
472 |
|
473 datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum); |
|
474 |
|
475 /* Go through all FMMU configs to detect data changes. */ |
|
476 list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) { |
|
477 |
|
478 if (fmmu->dir != EC_DIR_INPUT) { |
|
479 continue; |
|
480 } |
|
481 |
|
482 if (fmmu->logical_start_address >= |
|
483 logical_datagram_address + datagram_size) { |
|
484 // fmmu data contained in next datagram pair |
|
485 break; |
|
486 } |
|
487 |
|
488 datagram_offset = |
|
489 fmmu->logical_start_address - logical_datagram_address; |
|
490 |
|
491 #if DEBUG_REDUNDANCY |
|
492 EC_MASTER_DBG(domain->master, 1, |
|
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 } |
|
504 #endif |
|
505 |
|
506 if (data_changed(pair->send_buffer, main_datagram, |
|
507 datagram_offset, fmmu->data_size)) { |
|
508 /* data changed on main link: no copying necessary. */ |
|
509 #if DEBUG_REDUNDANCY |
|
510 EC_MASTER_DBG(domain->master, 1, "main changed\n"); |
|
511 #endif |
|
512 } else if (data_changed(pair->send_buffer, backup_datagram, |
|
513 datagram_offset, fmmu->data_size)) { |
|
514 /* data changed on backup link: copy to main memory. */ |
|
515 #if DEBUG_REDUNDANCY |
|
516 EC_MASTER_DBG(domain->master, 1, "backup changed\n"); |
|
517 #endif |
|
518 memcpy(main_datagram->data + datagram_offset, |
|
519 backup_datagram->data + datagram_offset, |
|
520 fmmu->data_size); |
|
521 } else if (datagram_pair_wc == pair->expected_working_counter) { |
|
522 /* no change, but WC complete: use main data. */ |
|
523 #if DEBUG_REDUNDANCY |
|
524 EC_MASTER_DBG(domain->master, 1, "no change but complete\n"); |
|
525 #endif |
|
526 } else { |
|
527 /* no change and WC incomplete: mark WC as zero to avoid |
|
528 * data.dependent WC flickering. */ |
|
529 datagram_pair_wc = 0; |
|
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])) { |
413 domain->working_counter_changes++; |
555 domain->working_counter_changes++; |
414 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]; |
415 } |
558 } |
416 |
559 |
417 if (domain->working_counter_changes && |
560 if (domain->working_counter_changes && |
418 jiffies - domain->notify_jiffies > HZ) { |
561 jiffies - domain->notify_jiffies > HZ) { |
419 domain->notify_jiffies = jiffies; |
562 domain->notify_jiffies = jiffies; |
420 if (domain->working_counter_changes == 1) { |
563 if (domain->working_counter_changes == 1) { |
421 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
564 EC_MASTER_INFO(domain->master, "Domain %u: Working counter" |
422 " changed to %u/%u.\n", domain->index, |
565 " changed to %u/%u (%u+%u).\n", domain->index, |
423 domain->working_counter, domain->expected_working_counter); |
566 domain->working_counter[EC_DEVICE_MAIN] + |
|
567 domain->working_counter[EC_DEVICE_BACKUP], |
|
568 domain->expected_working_counter, |
|
569 wc_sum[EC_DEVICE_MAIN], wc_sum[EC_DEVICE_BACKUP]); |
424 } else { |
570 } else { |
425 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
571 EC_MASTER_INFO(domain->master, "Domain %u: %u working counter" |
426 " changes - now %u/%u.\n", domain->index, |
572 " changes - now %u/%u (%u+%u).\n", domain->index, |
427 domain->working_counter_changes, domain->working_counter, |
573 domain->working_counter_changes, |
428 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]); |
429 } |
578 } |
430 domain->working_counter_changes = 0; |
579 domain->working_counter_changes = 0; |
431 } |
580 } |
432 } |
581 } |
433 |
582 |
434 /*****************************************************************************/ |
583 /*****************************************************************************/ |
435 |
584 |
436 void ecrt_domain_queue(ec_domain_t *domain) |
585 void ecrt_domain_queue(ec_domain_t *domain) |
437 { |
586 { |
438 ec_datagram_t *datagram; |
587 ec_datagram_pair_t *datagram_pair; |
439 |
588 ec_device_index_t dev_idx; |
440 list_for_each_entry(datagram, &domain->datagrams, list) { |
589 |
441 ec_master_queue_datagram(domain->master, datagram); |
590 list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) { |
|
591 |
|
592 /* copy main data to send buffer */ |
|
593 memcpy(datagram_pair->send_buffer, |
|
594 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
|
595 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
|
596 |
|
597 /* copy main data to backup datagram */ |
|
598 memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data, |
|
599 datagram_pair->datagrams[EC_DEVICE_MAIN].data, |
|
600 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size); |
|
601 |
|
602 for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) { |
|
603 ec_master_queue_datagram(domain->master, |
|
604 &datagram_pair->datagrams[dev_idx]); |
|
605 } |
442 } |
606 } |
443 } |
607 } |
444 |
608 |
445 /*****************************************************************************/ |
609 /*****************************************************************************/ |
446 |
610 |
447 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) |
448 { |
612 { |
449 state->working_counter = domain->working_counter; |
613 state->working_counter = |
450 |
614 domain->working_counter[EC_DEVICE_MAIN] |
451 if (domain->working_counter) { |
615 + domain->working_counter[EC_DEVICE_BACKUP]; |
452 if (domain->working_counter == domain->expected_working_counter) { |
616 |
|
617 if (state->working_counter) { |
|
618 if (state->working_counter == domain->expected_working_counter) { |
453 state->wc_state = EC_WC_COMPLETE; |
619 state->wc_state = EC_WC_COMPLETE; |
454 } else { |
620 } else { |
455 state->wc_state = EC_WC_INCOMPLETE; |
621 state->wc_state = EC_WC_INCOMPLETE; |
456 } |
622 } |
457 } else { |
623 } else { |
458 state->wc_state = EC_WC_ZERO; |
624 state->wc_state = EC_WC_ZERO; |
459 } |
625 } |
|
626 |
|
627 state->redundancy_active = domain->redundancy_active; |
460 } |
628 } |
461 |
629 |
462 /*****************************************************************************/ |
630 /*****************************************************************************/ |
463 |
631 |
464 /** \cond */ |
632 /** \cond */ |