188 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
188 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
189 return; |
189 return; |
190 |
190 |
191 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
191 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
192 fsm->state = ec_fsm_slave_scan_state_error; |
192 fsm->state = ec_fsm_slave_scan_state_error; |
193 EC_ERR("Failed to receive station address datagram for slave %u: ", |
193 EC_SLAVE_ERR(fsm->slave, |
194 fsm->slave->ring_position); |
194 "Failed to receive station address datagram: "); |
195 ec_datagram_print_state(datagram); |
195 ec_datagram_print_state(datagram); |
196 return; |
196 return; |
197 } |
197 } |
198 |
198 |
199 if (datagram->working_counter != 1) { |
199 if (datagram->working_counter != 1) { |
200 fsm->slave->error_flag = 1; |
200 fsm->slave->error_flag = 1; |
201 fsm->state = ec_fsm_slave_scan_state_error; |
201 fsm->state = ec_fsm_slave_scan_state_error; |
202 EC_ERR("Failed to write station address on slave %u: ", |
202 EC_SLAVE_ERR(fsm->slave, "Failed to write station address: "); |
203 fsm->slave->ring_position); |
|
204 ec_datagram_print_wc_error(datagram); |
203 ec_datagram_print_wc_error(datagram); |
205 return; |
204 return; |
206 } |
205 } |
207 |
206 |
208 // Read AL state |
207 // Read AL state |
228 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
227 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
229 return; |
228 return; |
230 |
229 |
231 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
230 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
232 fsm->state = ec_fsm_slave_scan_state_error; |
231 fsm->state = ec_fsm_slave_scan_state_error; |
233 EC_ERR("Failed to receive AL state datagram from slave %u: ", |
232 EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: "); |
234 fsm->slave->ring_position); |
|
235 ec_datagram_print_state(datagram); |
233 ec_datagram_print_state(datagram); |
236 return; |
234 return; |
237 } |
235 } |
238 |
236 |
239 if (datagram->working_counter != 1) { |
237 if (datagram->working_counter != 1) { |
240 fsm->slave->error_flag = 1; |
238 fsm->slave->error_flag = 1; |
241 fsm->state = ec_fsm_slave_scan_state_error; |
239 fsm->state = ec_fsm_slave_scan_state_error; |
242 EC_ERR("Failed to read AL state of slave %u: ", |
240 EC_SLAVE_ERR(slave, "Failed to read AL state: "); |
243 fsm->slave->ring_position); |
|
244 ec_datagram_print_wc_error(datagram); |
241 ec_datagram_print_wc_error(datagram); |
245 return; |
242 return; |
246 } |
243 } |
247 |
244 |
248 slave->current_state = EC_READ_U8(datagram->data); |
245 slave->current_state = EC_READ_U8(datagram->data); |
249 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
246 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
250 char state_str[EC_STATE_STRING_SIZE]; |
247 char state_str[EC_STATE_STRING_SIZE]; |
251 ec_state_string(slave->current_state, state_str, 0); |
248 ec_state_string(slave->current_state, state_str, 0); |
252 EC_WARN("Slave %u has state error bit set (%s)!\n", |
249 EC_SLAVE_WARN(slave, "Slave has state error bit set (%s)!\n", |
253 slave->ring_position, state_str); |
250 state_str); |
254 } |
251 } |
255 |
252 |
256 // read base data |
253 // read base data |
257 ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0000, 12); |
254 ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0000, 12); |
258 ec_datagram_zero(datagram); |
255 ec_datagram_zero(datagram); |
276 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
273 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
277 return; |
274 return; |
278 |
275 |
279 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
276 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
280 fsm->state = ec_fsm_slave_scan_state_error; |
277 fsm->state = ec_fsm_slave_scan_state_error; |
281 EC_ERR("Failed to receive base data datagram for slave %u: ", |
278 EC_SLAVE_ERR(slave, "Failed to receive base data datagram: "); |
282 slave->ring_position); |
|
283 ec_datagram_print_state(datagram); |
279 ec_datagram_print_state(datagram); |
284 return; |
280 return; |
285 } |
281 } |
286 |
282 |
287 if (datagram->working_counter != 1) { |
283 if (datagram->working_counter != 1) { |
288 fsm->slave->error_flag = 1; |
284 fsm->slave->error_flag = 1; |
289 fsm->state = ec_fsm_slave_scan_state_error; |
285 fsm->state = ec_fsm_slave_scan_state_error; |
290 EC_ERR("Failed to read base data from slave %u: ", |
286 EC_SLAVE_ERR(slave, "Failed to read base data: "); |
291 slave->ring_position); |
|
292 ec_datagram_print_wc_error(datagram); |
287 ec_datagram_print_wc_error(datagram); |
293 return; |
288 return; |
294 } |
289 } |
295 |
290 |
296 slave->base_type = EC_READ_U8 (datagram->data); |
291 slave->base_type = EC_READ_U8 (datagram->data); |
297 slave->base_revision = EC_READ_U8 (datagram->data + 1); |
292 slave->base_revision = EC_READ_U8 (datagram->data + 1); |
298 slave->base_build = EC_READ_U16(datagram->data + 2); |
293 slave->base_build = EC_READ_U16(datagram->data + 2); |
299 |
294 |
300 slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); |
295 slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); |
301 if (slave->base_fmmu_count > EC_MAX_FMMUS) { |
296 if (slave->base_fmmu_count > EC_MAX_FMMUS) { |
302 EC_WARN("Slave %u has more FMMUs (%u) than the master can" |
297 EC_SLAVE_WARN(slave, "Slave has more FMMUs (%u) than the master can" |
303 " handle (%u).\n", slave->ring_position, |
298 " handle (%u).\n", slave->base_fmmu_count, EC_MAX_FMMUS); |
304 slave->base_fmmu_count, EC_MAX_FMMUS); |
|
305 slave->base_fmmu_count = EC_MAX_FMMUS; |
299 slave->base_fmmu_count = EC_MAX_FMMUS; |
306 } |
300 } |
307 |
301 |
308 slave->base_sync_count = EC_READ_U8(datagram->data + 5); |
302 slave->base_sync_count = EC_READ_U8(datagram->data + 5); |
309 if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) { |
303 if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) { |
310 EC_WARN("Slave %u provides more sync managers (%u) than the master can" |
304 EC_SLAVE_WARN(slave, "Slave provides more sync managers (%u)" |
311 " handle (%u).\n", slave->ring_position, |
305 " than the master can handle (%u).\n", |
312 slave->base_sync_count, EC_MAX_SYNC_MANAGERS); |
306 slave->base_sync_count, EC_MAX_SYNC_MANAGERS); |
313 slave->base_sync_count = EC_MAX_SYNC_MANAGERS; |
307 slave->base_sync_count = EC_MAX_SYNC_MANAGERS; |
314 } |
308 } |
315 |
309 |
316 octet = EC_READ_U8(datagram->data + 7); |
310 octet = EC_READ_U8(datagram->data + 7); |
351 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
345 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
352 return; |
346 return; |
353 |
347 |
354 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
348 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
355 fsm->state = ec_fsm_slave_scan_state_error; |
349 fsm->state = ec_fsm_slave_scan_state_error; |
356 EC_ERR("Failed to receive system time datagram for slave %u: ", |
350 EC_SLAVE_ERR(slave, "Failed to receive system time datagram: "); |
357 slave->ring_position); |
|
358 ec_datagram_print_state(datagram); |
351 ec_datagram_print_state(datagram); |
359 return; |
352 return; |
360 } |
353 } |
361 |
354 |
362 if (datagram->working_counter == 1) { |
355 if (datagram->working_counter == 1) { |
363 slave->has_dc_system_time = 1; |
356 slave->has_dc_system_time = 1; |
364 if (slave->master->debug_level) { |
357 EC_SLAVE_DBG(slave, 1, "Slave has the System Time register.\n"); |
365 EC_DBG("Slave %u has the System Time register.\n", |
|
366 slave->ring_position); |
|
367 } |
|
368 } else if (datagram->working_counter == 0) { |
358 } else if (datagram->working_counter == 0) { |
369 if (slave->master->debug_level) { |
359 EC_SLAVE_DBG(slave, 1, "Slave has no System Time register; delay " |
370 EC_DBG("Slave %u has no System Time register; delay " |
360 "measurement only.\n"); |
371 "measurement only.\n", slave->ring_position); |
|
372 } |
|
373 } else { |
361 } else { |
374 fsm->slave->error_flag = 1; |
362 fsm->slave->error_flag = 1; |
375 fsm->state = ec_fsm_slave_scan_state_error; |
363 fsm->state = ec_fsm_slave_scan_state_error; |
376 EC_ERR("Failed to determine, if system time register is " |
364 EC_SLAVE_ERR(slave, "Failed to determine, if system time register is " |
377 "supported by slave %u: ", slave->ring_position); |
365 "supported: "); |
378 ec_datagram_print_wc_error(datagram); |
366 ec_datagram_print_wc_error(datagram); |
379 return; |
367 return; |
380 } |
368 } |
381 |
369 |
382 // read DC port receive times |
370 // read DC port receive times |
403 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
391 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
404 return; |
392 return; |
405 |
393 |
406 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
394 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
407 fsm->state = ec_fsm_slave_scan_state_error; |
395 fsm->state = ec_fsm_slave_scan_state_error; |
408 EC_ERR("Failed to receive system time datagram for slave %u: ", |
396 EC_SLAVE_ERR(slave, "Failed to receive system time datagram: "); |
409 slave->ring_position); |
|
410 ec_datagram_print_state(datagram); |
397 ec_datagram_print_state(datagram); |
411 return; |
398 return; |
412 } |
399 } |
413 |
400 |
414 if (datagram->working_counter != 1) { |
401 if (datagram->working_counter != 1) { |
415 fsm->slave->error_flag = 1; |
402 fsm->slave->error_flag = 1; |
416 fsm->state = ec_fsm_slave_scan_state_error; |
403 fsm->state = ec_fsm_slave_scan_state_error; |
417 EC_ERR("Failed to get DC receive times of slave %u: ", |
404 EC_SLAVE_ERR(slave, "Failed to get DC receive times: "); |
418 slave->ring_position); |
|
419 ec_datagram_print_wc_error(datagram); |
405 ec_datagram_print_wc_error(datagram); |
420 return; |
406 return; |
421 } |
407 } |
422 |
408 |
423 for (i = 0; i < EC_MAX_PORTS; i++) { |
409 for (i = 0; i < EC_MAX_PORTS; i++) { |
463 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
449 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
464 return; |
450 return; |
465 |
451 |
466 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
452 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
467 fsm->state = ec_fsm_slave_scan_state_error; |
453 fsm->state = ec_fsm_slave_scan_state_error; |
468 EC_ERR("Failed to receive DL status datagram from slave %u: ", |
454 EC_SLAVE_ERR(slave, "Failed to receive DL status datagram: "); |
469 slave->ring_position); |
|
470 ec_datagram_print_state(datagram); |
455 ec_datagram_print_state(datagram); |
471 return; |
456 return; |
472 } |
457 } |
473 |
458 |
474 if (datagram->working_counter != 1) { |
459 if (datagram->working_counter != 1) { |
475 fsm->slave->error_flag = 1; |
460 fsm->slave->error_flag = 1; |
476 fsm->state = ec_fsm_slave_scan_state_error; |
461 fsm->state = ec_fsm_slave_scan_state_error; |
477 EC_ERR("Failed to read DL status from slave %u: ", |
462 EC_SLAVE_ERR(slave, "Failed to read DL status: "); |
478 slave->ring_position); |
|
479 ec_datagram_print_wc_error(datagram); |
463 ec_datagram_print_wc_error(datagram); |
480 return; |
464 return; |
481 } |
465 } |
482 |
466 |
483 dl_status = EC_READ_U16(datagram->data); |
467 dl_status = EC_READ_U16(datagram->data); |
484 for (i = 0; i < EC_MAX_PORTS; i++) { |
468 for (i = 0; i < EC_MAX_PORTS; i++) { |
485 slave->ports[i].link.link_up = dl_status & (1 << (4 + i)) ? 1 : 0; |
469 slave->ports[i].link.link_up = |
486 slave->ports[i].link.loop_closed = dl_status & (1 << (8 + i * 2)) ? 1 : 0; |
470 dl_status & (1 << (4 + i)) ? 1 : 0; |
487 slave->ports[i].link.signal_detected = dl_status & (1 << (9 + i * 2)) ? 1 : 0; |
471 slave->ports[i].link.loop_closed = |
|
472 dl_status & (1 << (8 + i * 2)) ? 1 : 0; |
|
473 slave->ports[i].link.signal_detected = |
|
474 dl_status & (1 << (9 + i * 2)) ? 1 : 0; |
488 } |
475 } |
489 |
476 |
490 // Start fetching SII size |
477 // Start fetching SII size |
491 |
478 |
492 fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header |
479 fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header |
510 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
497 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
511 |
498 |
512 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
499 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
513 fsm->slave->error_flag = 1; |
500 fsm->slave->error_flag = 1; |
514 fsm->state = ec_fsm_slave_scan_state_error; |
501 fsm->state = ec_fsm_slave_scan_state_error; |
515 EC_ERR("Failed to read SII size of slave %u.\n", |
502 EC_SLAVE_ERR(slave, "Failed to read SII size.\n"); |
516 slave->ring_position); |
|
517 return; |
503 return; |
518 } |
504 } |
519 |
505 |
520 cat_type = EC_READ_U16(fsm->fsm_sii.value); |
506 cat_type = EC_READ_U16(fsm->fsm_sii.value); |
521 cat_size = EC_READ_U16(fsm->fsm_sii.value + 2); |
507 cat_size = EC_READ_U16(fsm->fsm_sii.value + 2); |
522 |
508 |
523 if (cat_type != 0xFFFF) { // not the last category |
509 if (cat_type != 0xFFFF) { // not the last category |
524 off_t next_offset = 2UL + fsm->sii_offset + cat_size; |
510 off_t next_offset = 2UL + fsm->sii_offset + cat_size; |
525 if (next_offset >= EC_MAX_SII_SIZE) { |
511 if (next_offset >= EC_MAX_SII_SIZE) { |
526 EC_WARN("SII size of slave %u exceeds" |
512 EC_SLAVE_WARN(slave, "SII size exceeds %u words" |
527 " %u words (0xffff limiter missing?).\n", |
513 " (0xffff limiter missing?).\n", EC_MAX_SII_SIZE); |
528 slave->ring_position, EC_MAX_SII_SIZE); |
|
529 // cut off category data... |
514 // cut off category data... |
530 slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET; |
515 slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET; |
531 goto alloc_sii; |
516 goto alloc_sii; |
532 } |
517 } |
533 fsm->sii_offset = next_offset; |
518 fsm->sii_offset = next_offset; |
539 |
524 |
540 slave->sii_nwords = fsm->sii_offset + 1; |
525 slave->sii_nwords = fsm->sii_offset + 1; |
541 |
526 |
542 alloc_sii: |
527 alloc_sii: |
543 if (slave->sii_words) { |
528 if (slave->sii_words) { |
544 EC_WARN("Freeing old SII data on slave %u...\n", |
529 EC_SLAVE_WARN(slave, "Freeing old SII data...\n"); |
545 slave->ring_position); |
|
546 kfree(slave->sii_words); |
530 kfree(slave->sii_words); |
547 } |
531 } |
548 |
532 |
549 if (!(slave->sii_words = |
533 if (!(slave->sii_words = |
550 (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_KERNEL))) { |
534 (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_KERNEL))) { |
551 EC_ERR("Failed to allocate %zu words of SII data for slave %u.\n", |
535 EC_SLAVE_ERR(slave, "Failed to allocate %zu words of SII data.\n", |
552 slave->sii_nwords, slave->ring_position); |
536 slave->sii_nwords); |
553 slave->sii_nwords = 0; |
537 slave->sii_nwords = 0; |
554 slave->error_flag = 1; |
538 slave->error_flag = 1; |
555 fsm->state = ec_fsm_slave_scan_state_error; |
539 fsm->state = ec_fsm_slave_scan_state_error; |
556 return; |
540 return; |
557 } |
541 } |
642 fsm->state = ec_fsm_slave_scan_state_end; |
625 fsm->state = ec_fsm_slave_scan_state_end; |
643 return; |
626 return; |
644 } |
627 } |
645 |
628 |
646 if (slave->sii_nwords < EC_FIRST_SII_CATEGORY_OFFSET + 1) { |
629 if (slave->sii_nwords < EC_FIRST_SII_CATEGORY_OFFSET + 1) { |
647 EC_ERR("Unexpected end of SII data in slave %u:" |
630 EC_SLAVE_ERR(slave, "Unexpected end of SII data:" |
648 " First category header missing.\n", |
631 " First category header missing.\n"); |
649 slave->ring_position); |
|
650 goto end; |
632 goto end; |
651 } |
633 } |
652 |
634 |
653 // evaluate category data |
635 // evaluate category data |
654 cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET; |
636 cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET; |
655 while (EC_READ_U16(cat_word) != 0xFFFF) { |
637 while (EC_READ_U16(cat_word) != 0xFFFF) { |
656 |
638 |
657 // type and size words must fit |
639 // type and size words must fit |
658 if (cat_word + 2 - slave->sii_words > slave->sii_nwords) { |
640 if (cat_word + 2 - slave->sii_words > slave->sii_nwords) { |
659 EC_ERR("Unexpected end of SII data in slave %u:" |
641 EC_SLAVE_ERR(slave, "Unexpected end of SII data:" |
660 " Category header incomplete.\n", |
642 " Category header incomplete.\n"); |
661 slave->ring_position); |
|
662 goto end; |
643 goto end; |
663 } |
644 } |
664 |
645 |
665 cat_type = EC_READ_U16(cat_word) & 0x7FFF; |
646 cat_type = EC_READ_U16(cat_word) & 0x7FFF; |
666 cat_size = EC_READ_U16(cat_word + 1); |
647 cat_size = EC_READ_U16(cat_word + 1); |
667 cat_word += 2; |
648 cat_word += 2; |
668 |
649 |
669 if (cat_word + cat_size - slave->sii_words > slave->sii_nwords) { |
650 if (cat_word + cat_size - slave->sii_words > slave->sii_nwords) { |
670 EC_WARN("Unexpected end of SII data in slave %u:" |
651 EC_SLAVE_WARN(slave, "Unexpected end of SII data:" |
671 " Category data incomplete.\n", |
652 " Category data incomplete.\n"); |
672 slave->ring_position); |
|
673 goto end; |
653 goto end; |
674 } |
654 } |
675 |
655 |
676 switch (cat_type) { |
656 switch (cat_type) { |
677 case 0x000A: |
657 case 0x000A: |
701 cat_size * 2, EC_DIR_OUTPUT)) // RxPDO |
681 cat_size * 2, EC_DIR_OUTPUT)) // RxPDO |
702 goto end; |
682 goto end; |
703 break; |
683 break; |
704 default: |
684 default: |
705 if (fsm->slave->master->debug_level) |
685 if (fsm->slave->master->debug_level) |
706 EC_WARN("Unknown category type 0x%04X in slave %u.\n", |
686 EC_SLAVE_WARN(slave, "Unknown category type 0x%04X.\n", |
707 cat_type, slave->ring_position); |
687 cat_type); |
708 } |
688 } |
709 |
689 |
710 cat_word += cat_size; |
690 cat_word += cat_size; |
711 if (cat_word - slave->sii_words >= slave->sii_nwords) { |
691 if (cat_word - slave->sii_words >= slave->sii_nwords) { |
712 EC_WARN("Unexpected end of SII data in slave %u:" |
692 EC_SLAVE_WARN(slave, "Unexpected end of SII data:" |
713 " Next category header missing.\n", |
693 " Next category header missing.\n"); |
714 slave->ring_position); |
|
715 goto end; |
694 goto end; |
716 } |
695 } |
717 } |
696 } |
718 |
697 |
719 ec_fsm_slave_scan_enter_regalias(fsm); |
698 ec_fsm_slave_scan_enter_regalias(fsm); |
720 return; |
699 return; |
721 |
700 |
722 end: |
701 end: |
723 EC_ERR("Failed to analyze category data.\n"); |
702 EC_SLAVE_ERR(slave, "Failed to analyze category data.\n"); |
724 fsm->slave->error_flag = 1; |
703 fsm->slave->error_flag = 1; |
725 fsm->state = ec_fsm_slave_scan_state_error; |
704 fsm->state = ec_fsm_slave_scan_state_error; |
726 } |
705 } |
727 |
706 |
728 |
707 |
738 { |
717 { |
739 ec_datagram_t *datagram = fsm->datagram; |
718 ec_datagram_t *datagram = fsm->datagram; |
740 ec_slave_t *slave = fsm->slave; |
719 ec_slave_t *slave = fsm->slave; |
741 |
720 |
742 // read alias from register |
721 // read alias from register |
743 if (slave->master->debug_level) |
722 EC_SLAVE_DBG(slave, 1, "Reading alias from register.\n"); |
744 EC_DBG("Reading alias from register of slave %u.\n", |
|
745 slave->ring_position); |
|
746 ec_datagram_fprd(datagram, slave->station_address, 0x0012, 2); |
723 ec_datagram_fprd(datagram, slave->station_address, 0x0012, 2); |
747 ec_datagram_zero(datagram); |
724 ec_datagram_zero(datagram); |
748 fsm->retries = EC_FSM_RETRIES; |
725 fsm->retries = EC_FSM_RETRIES; |
749 fsm->state = ec_fsm_slave_scan_state_regalias; |
726 fsm->state = ec_fsm_slave_scan_state_regalias; |
750 } |
727 } |
764 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
741 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
765 return; |
742 return; |
766 |
743 |
767 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
744 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
768 fsm->state = ec_fsm_slave_scan_state_error; |
745 fsm->state = ec_fsm_slave_scan_state_error; |
769 EC_ERR("Failed to receive get reg alias datagram from slave %u: ", |
746 EC_SLAVE_ERR(slave, "Failed to receive register alias datagram: "); |
770 fsm->slave->ring_position); |
|
771 ec_datagram_print_state(datagram); |
747 ec_datagram_print_state(datagram); |
772 return; |
748 return; |
773 } |
749 } |
774 |
750 |
775 if (datagram->working_counter != 1) { |
751 if (datagram->working_counter != 1) { |
776 EC_WARN("Failed to read reg alias of slave %u.\n", |
752 EC_SLAVE_DBG(slave, 1, "Failed to read register alias.\n"); |
777 fsm->slave->ring_position); |
|
778 } else { |
753 } else { |
779 slave->effective_alias = EC_READ_U16(datagram->data); |
754 slave->effective_alias = EC_READ_U16(datagram->data); |
780 if (slave->master->debug_level) |
755 EC_SLAVE_DBG(slave, 1, "Read alias %u from register.\n", |
781 EC_DBG("Read alias %u from register of slave %u.\n", |
756 slave->effective_alias); |
782 slave->effective_alias, slave->ring_position); |
|
783 } |
757 } |
784 if (slave->sii.mailbox_protocols & EC_MBOX_COE) { |
758 if (slave->sii.mailbox_protocols & EC_MBOX_COE) { |
785 ec_fsm_slave_scan_enter_preop(fsm); |
759 ec_fsm_slave_scan_enter_preop(fsm); |
786 } else { |
760 } else { |
787 fsm->state = ec_fsm_slave_scan_state_end; |
761 fsm->state = ec_fsm_slave_scan_state_end; |
803 && current_state != EC_SLAVE_STATE_SAFEOP |
777 && current_state != EC_SLAVE_STATE_SAFEOP |
804 && current_state != EC_SLAVE_STATE_OP) { |
778 && current_state != EC_SLAVE_STATE_OP) { |
805 if (slave->master->debug_level) { |
779 if (slave->master->debug_level) { |
806 char str[EC_STATE_STRING_SIZE]; |
780 char str[EC_STATE_STRING_SIZE]; |
807 ec_state_string(current_state, str, 0); |
781 ec_state_string(current_state, str, 0); |
808 EC_DBG("Slave %u is not in the state to do mailbox com (%s)," |
782 EC_SLAVE_DBG(slave, 0, "Slave is not in the state" |
809 " setting to PREOP.\n", slave->ring_position, str); |
783 " to do mailbox com (%s), setting to PREOP.\n", str); |
810 } |
784 } |
811 |
785 |
812 fsm->state = ec_fsm_slave_scan_state_preop; |
786 fsm->state = ec_fsm_slave_scan_state_preop; |
813 ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); |
787 ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); |
814 ec_fsm_slave_config_start(fsm->fsm_slave_config, slave); |
788 ec_fsm_slave_config_start(fsm->fsm_slave_config, slave); |
815 ec_fsm_slave_config_exec(fsm->fsm_slave_config); |
789 ec_fsm_slave_config_exec(fsm->fsm_slave_config); |
816 } else { |
790 } else { |
817 if (slave->master->debug_level) |
791 EC_SLAVE_DBG(slave, 1, "Reading mailbox" |
818 EC_DBG("Reading mailbox syncmanager configuration of slave %u.\n", |
792 " sync manager configuration.\n"); |
819 slave->ring_position); |
|
820 |
793 |
821 /* Scan current sync manager configuration to get configured mailbox |
794 /* Scan current sync manager configuration to get configured mailbox |
822 * sizes. */ |
795 * sizes. */ |
823 ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800, |
796 ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800, |
824 EC_SYNC_PAGE_SIZE * 2); |
797 EC_SYNC_PAGE_SIZE * 2); |
860 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
833 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
861 return; |
834 return; |
862 |
835 |
863 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
836 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
864 fsm->state = ec_fsm_slave_scan_state_error; |
837 fsm->state = ec_fsm_slave_scan_state_error; |
865 EC_ERR("Failed to receive sync manager configuration datagram" |
838 EC_SLAVE_ERR(slave, "Failed to receive sync manager" |
866 " from slave %u: ", slave->ring_position); |
839 " configuration datagram: "); |
867 ec_datagram_print_state(datagram); |
840 ec_datagram_print_state(datagram); |
868 return; |
841 return; |
869 } |
842 } |
870 |
843 |
871 if (datagram->working_counter != 1) { |
844 if (datagram->working_counter != 1) { |
872 fsm->slave->error_flag = 1; |
845 fsm->slave->error_flag = 1; |
873 fsm->state = ec_fsm_slave_scan_state_error; |
846 fsm->state = ec_fsm_slave_scan_state_error; |
874 EC_ERR("Failed to read DL status from slave %u: ", |
847 EC_SLAVE_ERR(slave, "Failed to read DL status: "); |
875 slave->ring_position); |
|
876 ec_datagram_print_wc_error(datagram); |
848 ec_datagram_print_wc_error(datagram); |
877 return; |
849 return; |
878 } |
850 } |
879 |
851 |
880 slave->configured_rx_mailbox_offset = EC_READ_U16(datagram->data); |
852 slave->configured_rx_mailbox_offset = EC_READ_U16(datagram->data); |
881 slave->configured_rx_mailbox_size = EC_READ_U16(datagram->data + 2); |
853 slave->configured_rx_mailbox_size = EC_READ_U16(datagram->data + 2); |
882 slave->configured_tx_mailbox_offset = EC_READ_U16(datagram->data + 8); |
854 slave->configured_tx_mailbox_offset = EC_READ_U16(datagram->data + 8); |
883 slave->configured_tx_mailbox_size = EC_READ_U16(datagram->data + 10); |
855 slave->configured_tx_mailbox_size = EC_READ_U16(datagram->data + 10); |
884 |
856 |
885 if (slave->master->debug_level) { |
857 EC_SLAVE_DBG(slave, 1, "Mailbox configuration:\n"); |
886 EC_DBG("Mailbox configuration of slave %u:\n", slave->ring_position); |
858 EC_SLAVE_DBG(slave, 1, " RX offset=0x%04x size=%u\n", |
887 EC_DBG(" RX offset=0x%04x size=%u\n", |
859 slave->configured_rx_mailbox_offset, |
888 slave->configured_rx_mailbox_offset, |
860 slave->configured_rx_mailbox_size); |
889 slave->configured_rx_mailbox_size); |
861 EC_SLAVE_DBG(slave, 1, " TX offset=0x%04x size=%u\n", |
890 EC_DBG(" TX offset=0x%04x size=%u\n", |
862 slave->configured_tx_mailbox_offset, |
891 slave->configured_tx_mailbox_offset, |
863 slave->configured_tx_mailbox_size); |
892 slave->configured_tx_mailbox_size); |
|
893 } |
|
894 |
864 |
895 ec_fsm_slave_scan_enter_pdos(fsm); |
865 ec_fsm_slave_scan_enter_pdos(fsm); |
896 } |
866 } |
897 |
867 |
898 /*****************************************************************************/ |
868 /*****************************************************************************/ |
903 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
873 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
904 ) |
874 ) |
905 { |
875 { |
906 ec_slave_t *slave = fsm->slave; |
876 ec_slave_t *slave = fsm->slave; |
907 |
877 |
908 if (slave->master->debug_level) |
878 EC_SLAVE_DBG(slave, 1, "Scanning PDO assignment and mapping.\n"); |
909 EC_DBG("Scanning PDO assignment and mapping of slave %u.\n", |
|
910 slave->ring_position); |
|
911 fsm->state = ec_fsm_slave_scan_state_pdos; |
879 fsm->state = ec_fsm_slave_scan_state_pdos; |
912 ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave); |
880 ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave); |
913 ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately |
881 ec_fsm_pdo_exec(fsm->fsm_pdo); // execute immediately |
914 } |
882 } |
915 |
883 |