221 int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */) |
221 int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */) |
222 { |
222 { |
223 return fsm->state == ec_fsm_coe_end; |
223 return fsm->state == ec_fsm_coe_end; |
224 } |
224 } |
225 |
225 |
|
226 /*****************************************************************************/ |
|
227 |
|
228 /** |
|
229 */ |
|
230 int ec_fsm_coe_check_emergency( |
|
231 ec_fsm_coe_t *fsm, /**< Finite state machine */ |
|
232 const uint8_t *data, /**< CoE mailbox data. */ |
|
233 size_t size /**< CoE mailbox data size. */ |
|
234 ) |
|
235 { |
|
236 if (size < 2 || ((EC_READ_U16(data) >> 12) & 0x0F) != 0x01) |
|
237 return 0; |
|
238 |
|
239 if (size < 10) { |
|
240 EC_WARN("Received incomplete CoE Emergency request from slave %u:\n", |
|
241 fsm->slave->ring_position); |
|
242 ec_print_data(data, size); |
|
243 return 1; |
|
244 } |
|
245 |
|
246 EC_INFO("CoE Emergency Request received from slave %u:\n", |
|
247 fsm->slave->ring_position); |
|
248 EC_INFO("Error code 0x%04X, Error register 0x%02X, data:\n", |
|
249 EC_READ_U16(data + 2), EC_READ_U8(data + 4)); |
|
250 ec_print_data(data + 5, 5); |
|
251 return 1; |
|
252 } |
|
253 |
226 /****************************************************************************** |
254 /****************************************************************************** |
227 * CoE dictionary state machine |
255 * CoE dictionary state machine |
228 *****************************************************************************/ |
256 *****************************************************************************/ |
229 |
257 |
230 /** |
258 /** |
274 return; // FIXME: request again? |
302 return; // FIXME: request again? |
275 |
303 |
276 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
304 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
277 fsm->state = ec_fsm_coe_error; |
305 fsm->state = ec_fsm_coe_error; |
278 EC_ERR("Failed to receive CoE dictionary request datagram for" |
306 EC_ERR("Failed to receive CoE dictionary request datagram for" |
279 " slave %i (datagram state %i).\n", |
307 " slave %u (datagram state %i).\n", |
280 slave->ring_position, datagram->state); |
308 slave->ring_position, datagram->state); |
281 return; |
309 return; |
282 } |
310 } |
283 |
311 |
284 if (datagram->working_counter != 1) { |
312 if (datagram->working_counter != 1) { |
285 fsm->state = ec_fsm_coe_error; |
313 fsm->state = ec_fsm_coe_error; |
286 EC_ERR("Reception of CoE dictionary request failed on slave %i: ", |
314 EC_ERR("Reception of CoE dictionary request failed on slave %u: ", |
287 slave->ring_position); |
315 slave->ring_position); |
288 ec_datagram_print_wc_error(datagram); |
316 ec_datagram_print_wc_error(datagram); |
289 return; |
317 return; |
290 } |
318 } |
291 |
319 |
310 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
338 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
311 return; |
339 return; |
312 |
340 |
313 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
341 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
314 fsm->state = ec_fsm_coe_error; |
342 fsm->state = ec_fsm_coe_error; |
315 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
343 EC_ERR("Failed to receive CoE mailbox check datagram for slave %u" |
316 " (datagram state %i).\n", |
344 " (datagram state %i).\n", |
317 slave->ring_position, datagram->state); |
345 slave->ring_position, datagram->state); |
318 return; |
346 return; |
319 } |
347 } |
320 |
348 |
321 if (datagram->working_counter != 1) { |
349 if (datagram->working_counter != 1) { |
322 fsm->state = ec_fsm_coe_error; |
350 fsm->state = ec_fsm_coe_error; |
323 EC_ERR("Reception of CoE mailbox check datagram failed on slave %i: ", |
351 EC_ERR("Reception of CoE mailbox check datagram failed on slave %u: ", |
324 slave->ring_position); |
352 slave->ring_position); |
325 ec_datagram_print_wc_error(datagram); |
353 ec_datagram_print_wc_error(datagram); |
326 return; |
354 return; |
327 } |
355 } |
328 |
356 |
329 if (!ec_slave_mbox_check(datagram)) { |
357 if (!ec_slave_mbox_check(datagram)) { |
330 if (datagram->cycles_received |
358 if (datagram->cycles_received |
331 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
359 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
332 fsm->state = ec_fsm_coe_error; |
360 fsm->state = ec_fsm_coe_error; |
333 EC_ERR("Timeout while checking Sdo dictionary on slave %i.\n", |
361 EC_ERR("Timeout while checking Sdo dictionary on slave %u.\n", |
334 slave->ring_position); |
362 slave->ring_position); |
335 return; |
363 return; |
336 } |
364 } |
337 |
365 |
338 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
366 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
367 return; // FIXME: request again? |
395 return; // FIXME: request again? |
368 |
396 |
369 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
397 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
370 fsm->state = ec_fsm_coe_error; |
398 fsm->state = ec_fsm_coe_error; |
371 EC_ERR("Failed to receive CoE dictionary response datagram for" |
399 EC_ERR("Failed to receive CoE dictionary response datagram for" |
372 " slave %i (datagram state %i).\n", |
400 " slave %u (datagram state %i).\n", |
373 slave->ring_position, datagram->state); |
401 slave->ring_position, datagram->state); |
374 return; |
402 return; |
375 } |
403 } |
376 |
404 |
377 if (datagram->working_counter != 1) { |
405 if (datagram->working_counter != 1) { |
378 fsm->state = ec_fsm_coe_error; |
406 fsm->state = ec_fsm_coe_error; |
379 EC_ERR("Reception of CoE dictionary response failed on slave %i: ", |
407 EC_ERR("Reception of CoE dictionary response failed on slave %u: ", |
380 slave->ring_position); |
408 slave->ring_position); |
381 ec_datagram_print_wc_error(datagram); |
409 ec_datagram_print_wc_error(datagram); |
382 return; |
410 return; |
383 } |
411 } |
384 |
412 |
392 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
420 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
393 fsm->state = ec_fsm_coe_error; |
421 fsm->state = ec_fsm_coe_error; |
394 return; |
422 return; |
395 } |
423 } |
396 |
424 |
|
425 if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { |
|
426 // check for CoE response again |
|
427 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
428 fsm->retries = EC_FSM_RETRIES; |
|
429 fsm->state = ec_fsm_coe_dict_check; |
|
430 return; |
|
431 } |
|
432 |
397 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
433 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
398 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
434 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
399 EC_ERR("Sdo information error response at slave %i!\n", |
435 EC_ERR("Sdo information error response at slave %u!\n", |
400 slave->ring_position); |
436 slave->ring_position); |
401 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
437 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
402 fsm->state = ec_fsm_coe_error; |
438 fsm->state = ec_fsm_coe_error; |
403 return; |
439 return; |
404 } |
440 } |
405 |
441 |
406 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
442 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
407 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
443 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
408 EC_ERR("Invalid Sdo list response at slave %i!\n", |
444 EC_ERR("Invalid Sdo list response at slave %u!\n", |
409 slave->ring_position); |
445 slave->ring_position); |
410 ec_print_data(data, rec_size); |
446 ec_print_data(data, rec_size); |
411 fsm->state = ec_fsm_coe_error; |
447 fsm->state = ec_fsm_coe_error; |
412 return; |
448 return; |
413 } |
449 } |
498 return; // FIXME: check for response first? |
534 return; // FIXME: check for response first? |
499 |
535 |
500 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
536 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
501 fsm->state = ec_fsm_coe_error; |
537 fsm->state = ec_fsm_coe_error; |
502 EC_ERR("Failed to receive CoE Sdo description request datagram for" |
538 EC_ERR("Failed to receive CoE Sdo description request datagram for" |
503 " slave %i (datagram state %i).\n", |
539 " slave %u (datagram state %i).\n", |
504 slave->ring_position, datagram->state); |
540 slave->ring_position, datagram->state); |
505 return; |
541 return; |
506 } |
542 } |
507 |
543 |
508 if (datagram->working_counter != 1) { |
544 if (datagram->working_counter != 1) { |
509 fsm->state = ec_fsm_coe_error; |
545 fsm->state = ec_fsm_coe_error; |
510 EC_ERR("Reception of CoE Sdo description" |
546 EC_ERR("Reception of CoE Sdo description" |
511 " request failed on slave %i: ", slave->ring_position); |
547 " request failed on slave %u: ", slave->ring_position); |
512 ec_datagram_print_wc_error(datagram); |
548 ec_datagram_print_wc_error(datagram); |
513 return; |
549 return; |
514 } |
550 } |
515 |
551 |
516 fsm->cycles_start = datagram->cycles_sent; |
552 fsm->cycles_start = datagram->cycles_sent; |
534 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
570 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
535 return; |
571 return; |
536 |
572 |
537 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
573 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
538 fsm->state = ec_fsm_coe_error; |
574 fsm->state = ec_fsm_coe_error; |
539 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
575 EC_ERR("Failed to receive CoE mailbox check datagram from slave %u" |
540 " (datagram state %i).\n", |
576 " (datagram state %i).\n", |
541 slave->ring_position, datagram->state); |
577 slave->ring_position, datagram->state); |
542 return; |
578 return; |
543 } |
579 } |
544 |
580 |
545 if (datagram->working_counter != 1) { |
581 if (datagram->working_counter != 1) { |
546 fsm->state = ec_fsm_coe_error; |
582 fsm->state = ec_fsm_coe_error; |
547 EC_ERR("Reception of CoE mailbox check" |
583 EC_ERR("Reception of CoE mailbox check" |
548 " datagram failed on slave %i: ", slave->ring_position); |
584 " datagram failed on slave %u: ", slave->ring_position); |
549 ec_datagram_print_wc_error(datagram); |
585 ec_datagram_print_wc_error(datagram); |
550 return; |
586 return; |
551 } |
587 } |
552 |
588 |
553 if (!ec_slave_mbox_check(datagram)) { |
589 if (!ec_slave_mbox_check(datagram)) { |
554 if (datagram->cycles_received |
590 if (datagram->cycles_received |
555 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
591 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
556 fsm->state = ec_fsm_coe_error; |
592 fsm->state = ec_fsm_coe_error; |
557 EC_ERR("Timeout while checking Sdo description on slave %i.\n", |
593 EC_ERR("Timeout while checking Sdo description on slave %u.\n", |
558 slave->ring_position); |
594 slave->ring_position); |
559 return; |
595 return; |
560 } |
596 } |
561 |
597 |
562 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
598 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
590 return; // FIXME: request again? |
626 return; // FIXME: request again? |
591 |
627 |
592 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
628 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
593 fsm->state = ec_fsm_coe_error; |
629 fsm->state = ec_fsm_coe_error; |
594 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
630 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
595 " slave %i (datagram state %i).\n", |
631 " slave %u (datagram state %i).\n", |
596 slave->ring_position, datagram->state); |
632 slave->ring_position, datagram->state); |
597 return; |
633 return; |
598 } |
634 } |
599 |
635 |
600 if (datagram->working_counter != 1) { |
636 if (datagram->working_counter != 1) { |
601 fsm->state = ec_fsm_coe_error; |
637 fsm->state = ec_fsm_coe_error; |
602 EC_ERR("Reception of CoE Sdo description" |
638 EC_ERR("Reception of CoE Sdo description" |
603 " response failed on slave %i: ", slave->ring_position); |
639 " response failed on slave %u: ", slave->ring_position); |
604 ec_datagram_print_wc_error(datagram); |
640 ec_datagram_print_wc_error(datagram); |
605 return; |
641 return; |
606 } |
642 } |
607 |
643 |
608 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
644 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
612 } |
648 } |
613 |
649 |
614 if (mbox_prot != 0x03) { // CoE |
650 if (mbox_prot != 0x03) { // CoE |
615 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
651 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
616 fsm->state = ec_fsm_coe_error; |
652 fsm->state = ec_fsm_coe_error; |
617 return; |
653 return; |
|
654 } |
|
655 |
|
656 if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { |
|
657 // check for CoE response again |
|
658 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
659 fsm->retries = EC_FSM_RETRIES; |
|
660 fsm->state = ec_fsm_coe_dict_desc_check; |
|
661 return; |
618 } |
662 } |
619 |
663 |
620 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
664 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
621 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
665 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
622 EC_ERR("Sdo information error response at slave %i while" |
666 EC_ERR("Sdo information error response at slave %u while" |
623 " fetching Sdo 0x%04X!\n", slave->ring_position, |
667 " fetching Sdo 0x%04X!\n", slave->ring_position, |
624 sdo->index); |
668 sdo->index); |
625 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
669 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
626 fsm->state = ec_fsm_coe_error; |
670 fsm->state = ec_fsm_coe_error; |
627 return; |
671 return; |
628 } |
672 } |
629 |
673 |
630 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
674 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
631 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
675 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
632 EC_READ_U16(data + 6) != sdo->index) { // Sdo index |
676 EC_READ_U16(data + 6) != sdo->index) { // Sdo index |
633 EC_ERR("Invalid object description response at slave %i while" |
677 EC_ERR("Invalid object description response at slave %u while" |
634 " fetching Sdo 0x%04X!\n", slave->ring_position, |
678 " fetching Sdo 0x%04X!\n", slave->ring_position, |
635 sdo->index); |
679 sdo->index); |
636 ec_print_data(data, rec_size); |
680 ec_print_data(data, rec_size); |
637 fsm->state = ec_fsm_coe_error; |
681 fsm->state = ec_fsm_coe_error; |
638 return; |
682 return; |
639 } |
683 } |
640 |
684 |
641 if (rec_size < 12) { |
685 if (rec_size < 12) { |
642 EC_ERR("Invalid data size!\n"); |
686 EC_ERR("Invalid data size!\n"); |
643 ec_print_data(data, rec_size); |
687 ec_print_data(data, rec_size); |
644 fsm->state = ec_fsm_coe_error; |
688 fsm->state = ec_fsm_coe_error; |
645 return; |
689 return; |
646 } |
690 } |
647 |
691 |
648 sdo->subindices = EC_READ_U8(data + 10); |
692 sdo->subindices = EC_READ_U8(data + 10); |
649 sdo->object_code = EC_READ_U8(data + 11); |
693 sdo->object_code = EC_READ_U8(data + 11); |
650 |
694 |
704 return; // FIXME: check for response first? |
748 return; // FIXME: check for response first? |
705 |
749 |
706 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
750 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
707 fsm->state = ec_fsm_coe_error; |
751 fsm->state = ec_fsm_coe_error; |
708 EC_ERR("Failed to receive CoE Sdo entry request datagram for" |
752 EC_ERR("Failed to receive CoE Sdo entry request datagram for" |
709 " slave %i (datagram state %i).\n", |
753 " slave %u (datagram state %i).\n", |
710 slave->ring_position, datagram->state); |
754 slave->ring_position, datagram->state); |
711 return; |
755 return; |
712 } |
756 } |
713 |
757 |
714 if (datagram->working_counter != 1) { |
758 if (datagram->working_counter != 1) { |
715 fsm->state = ec_fsm_coe_error; |
759 fsm->state = ec_fsm_coe_error; |
716 EC_ERR("Reception of CoE Sdo entry request failed on slave %i: ", |
760 EC_ERR("Reception of CoE Sdo entry request failed on slave %u: ", |
717 slave->ring_position); |
761 slave->ring_position); |
718 ec_datagram_print_wc_error(datagram); |
762 ec_datagram_print_wc_error(datagram); |
719 return; |
763 return; |
720 } |
764 } |
721 |
765 |
741 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
785 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
742 return; |
786 return; |
743 |
787 |
744 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
788 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
745 fsm->state = ec_fsm_coe_error; |
789 fsm->state = ec_fsm_coe_error; |
746 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
790 EC_ERR("Failed to receive CoE mailbox check datagram from slave %u" |
747 " (datagram state %i).\n", |
791 " (datagram state %i).\n", |
748 slave->ring_position, datagram->state); |
792 slave->ring_position, datagram->state); |
749 return; |
793 return; |
750 } |
794 } |
751 |
795 |
752 if (datagram->working_counter != 1) { |
796 if (datagram->working_counter != 1) { |
753 fsm->state = ec_fsm_coe_error; |
797 fsm->state = ec_fsm_coe_error; |
754 EC_ERR("Reception of CoE mailbox check" |
798 EC_ERR("Reception of CoE mailbox check" |
755 " datagram failed on slave %i: ", slave->ring_position); |
799 " datagram failed on slave %u: ", slave->ring_position); |
756 ec_datagram_print_wc_error(datagram); |
800 ec_datagram_print_wc_error(datagram); |
757 return; |
801 return; |
758 } |
802 } |
759 |
803 |
760 if (!ec_slave_mbox_check(datagram)) { |
804 if (!ec_slave_mbox_check(datagram)) { |
761 if (datagram->cycles_received |
805 if (datagram->cycles_received |
762 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
806 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
763 fsm->state = ec_fsm_coe_error; |
807 fsm->state = ec_fsm_coe_error; |
764 EC_ERR("Timeout while checking Sdo entry on slave %i.\n", |
808 EC_ERR("Timeout while checking Sdo entry on slave %u.\n", |
765 slave->ring_position); |
809 slave->ring_position); |
766 return; |
810 return; |
767 } |
811 } |
768 |
812 |
769 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
813 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
798 return; // FIXME: request again? |
842 return; // FIXME: request again? |
799 |
843 |
800 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
844 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
801 fsm->state = ec_fsm_coe_error; |
845 fsm->state = ec_fsm_coe_error; |
802 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
846 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
803 " slave %i (datagram state %i).\n", |
847 " slave %u (datagram state %i).\n", |
804 slave->ring_position, datagram->state); |
848 slave->ring_position, datagram->state); |
805 return; |
849 return; |
806 } |
850 } |
807 |
851 |
808 if (datagram->working_counter != 1) { |
852 if (datagram->working_counter != 1) { |
809 fsm->state = ec_fsm_coe_error; |
853 fsm->state = ec_fsm_coe_error; |
810 EC_ERR("Reception of CoE Sdo description" |
854 EC_ERR("Reception of CoE Sdo description" |
811 " response failed on slave %i: ", slave->ring_position); |
855 " response failed on slave %u: ", slave->ring_position); |
812 ec_datagram_print_wc_error(datagram); |
856 ec_datagram_print_wc_error(datagram); |
813 return; |
857 return; |
814 } |
858 } |
815 |
859 |
816 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
860 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
820 } |
864 } |
821 |
865 |
822 if (mbox_prot != 0x03) { // CoE |
866 if (mbox_prot != 0x03) { // CoE |
823 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
867 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
824 fsm->state = ec_fsm_coe_error; |
868 fsm->state = ec_fsm_coe_error; |
825 return; |
869 return; |
|
870 } |
|
871 |
|
872 if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { |
|
873 // check for CoE response again |
|
874 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
875 fsm->retries = EC_FSM_RETRIES; |
|
876 fsm->state = ec_fsm_coe_dict_entry_check; |
|
877 return; |
826 } |
878 } |
827 |
879 |
828 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
880 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
829 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
881 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
830 EC_ERR("Sdo information error response at slave %i while" |
882 EC_ERR("Sdo information error response at slave %u while" |
831 " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position, |
883 " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position, |
832 sdo->index, fsm->subindex); |
884 sdo->index, fsm->subindex); |
833 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
885 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
834 fsm->state = ec_fsm_coe_error; |
886 fsm->state = ec_fsm_coe_error; |
835 return; |
887 return; |
836 } |
888 } |
837 |
889 |
838 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
890 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
839 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
891 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
840 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
892 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
841 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
893 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
842 EC_ERR("Invalid entry description response at slave %i while" |
894 EC_ERR("Invalid entry description response at slave %u while" |
843 " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position, |
895 " fetching Sdo entry 0x%04X:%u!\n", slave->ring_position, |
844 sdo->index, fsm->subindex); |
896 sdo->index, fsm->subindex); |
845 ec_print_data(data, rec_size); |
897 ec_print_data(data, rec_size); |
846 fsm->state = ec_fsm_coe_error; |
898 fsm->state = ec_fsm_coe_error; |
847 return; |
899 return; |
848 } |
900 } |
849 |
901 |
850 if (rec_size < 16) { |
902 if (rec_size < 16) { |
851 EC_ERR("Invalid data size!\n"); |
903 EC_ERR("Invalid data size!\n"); |
852 ec_print_data(data, rec_size); |
904 ec_print_data(data, rec_size); |
853 fsm->state = ec_fsm_coe_error; |
905 fsm->state = ec_fsm_coe_error; |
854 return; |
906 return; |
855 } |
907 } |
856 |
908 |
857 data_size = rec_size - 16; |
909 data_size = rec_size - 16; |
858 |
910 |
859 if (!(entry = (ec_sdo_entry_t *) |
911 if (!(entry = (ec_sdo_entry_t *) |
860 kmalloc(sizeof(ec_sdo_entry_t), GFP_ATOMIC))) { |
912 kmalloc(sizeof(ec_sdo_entry_t), GFP_ATOMIC))) { |
861 EC_ERR("Failed to allocate entry!\n"); |
913 EC_ERR("Failed to allocate entry!\n"); |
862 fsm->state = ec_fsm_coe_error; |
914 fsm->state = ec_fsm_coe_error; |
863 return; |
915 return; |
864 } |
916 } |
865 |
917 |
866 if (ec_sdo_entry_init(entry, fsm->subindex, sdo)) { |
918 if (ec_sdo_entry_init(entry, fsm->subindex, sdo)) { |
867 EC_ERR("Failed to init entry!\n"); |
919 EC_ERR("Failed to init entry!\n"); |
868 fsm->state = ec_fsm_coe_error; |
920 fsm->state = ec_fsm_coe_error; |
869 return; |
921 return; |
870 } |
922 } |
871 |
923 |
872 entry->data_type = EC_READ_U16(data + 10); |
924 entry->data_type = EC_READ_U16(data + 10); |
873 entry->bit_length = EC_READ_U16(data + 12); |
925 entry->bit_length = EC_READ_U16(data + 12); |
874 |
926 |
997 return; // FIXME: check for response first? |
1049 return; // FIXME: check for response first? |
998 |
1050 |
999 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1051 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1000 fsm->state = ec_fsm_coe_error; |
1052 fsm->state = ec_fsm_coe_error; |
1001 EC_ERR("Failed to receive CoE download request datagram for" |
1053 EC_ERR("Failed to receive CoE download request datagram for" |
1002 " slave %i (datagram state %i).\n", |
1054 " slave %u (datagram state %i).\n", |
1003 slave->ring_position, datagram->state); |
1055 slave->ring_position, datagram->state); |
1004 return; |
1056 return; |
1005 } |
1057 } |
1006 |
1058 |
1007 if (datagram->working_counter != 1) { |
1059 if (datagram->working_counter != 1) { |
1008 fsm->state = ec_fsm_coe_error; |
1060 fsm->state = ec_fsm_coe_error; |
1009 EC_ERR("Reception of CoE download request failed on slave %i: ", |
1061 EC_ERR("Reception of CoE download request failed on slave %u: ", |
1010 slave->ring_position); |
1062 slave->ring_position); |
1011 ec_datagram_print_wc_error(datagram); |
1063 ec_datagram_print_wc_error(datagram); |
1012 return; |
1064 return; |
1013 } |
1065 } |
1014 |
1066 |
1033 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1085 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1034 return; |
1086 return; |
1035 |
1087 |
1036 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1088 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1037 fsm->state = ec_fsm_coe_error; |
1089 fsm->state = ec_fsm_coe_error; |
1038 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1090 EC_ERR("Failed to receive CoE mailbox check datagram for slave %u" |
1039 " (datagram state %i).\n", |
1091 " (datagram state %i).\n", |
1040 slave->ring_position, datagram->state); |
1092 slave->ring_position, datagram->state); |
1041 return; |
1093 return; |
1042 } |
1094 } |
1043 |
1095 |
1044 if (datagram->working_counter != 1) { |
1096 if (datagram->working_counter != 1) { |
1045 fsm->state = ec_fsm_coe_error; |
1097 fsm->state = ec_fsm_coe_error; |
1046 EC_ERR("Reception of CoE mailbox check" |
1098 EC_ERR("Reception of CoE mailbox check" |
1047 " datagram failed on slave %i: ", slave->ring_position); |
1099 " datagram failed on slave %u: ", slave->ring_position); |
1048 ec_datagram_print_wc_error(datagram); |
1100 ec_datagram_print_wc_error(datagram); |
1049 return; |
1101 return; |
1050 } |
1102 } |
1051 |
1103 |
1052 if (!ec_slave_mbox_check(datagram)) { |
1104 if (!ec_slave_mbox_check(datagram)) { |
1053 if (datagram->cycles_received |
1105 if (datagram->cycles_received |
1054 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1106 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1055 fsm->state = ec_fsm_coe_error; |
1107 fsm->state = ec_fsm_coe_error; |
1056 EC_ERR("Timeout while checking Sdo configuration on slave %i.\n", |
1108 EC_ERR("Timeout while checking Sdo configuration on slave %u.\n", |
1057 slave->ring_position); |
1109 slave->ring_position); |
1058 return; |
1110 return; |
1059 } |
1111 } |
1060 |
1112 |
1061 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1113 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1088 return; // FIXME: request again? |
1140 return; // FIXME: request again? |
1089 |
1141 |
1090 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1142 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1091 fsm->state = ec_fsm_coe_error; |
1143 fsm->state = ec_fsm_coe_error; |
1092 EC_ERR("Failed to receive CoE download response datagram from" |
1144 EC_ERR("Failed to receive CoE download response datagram from" |
1093 " slave %i (datagram state %i).\n", |
1145 " slave %u (datagram state %i).\n", |
1094 slave->ring_position, datagram->state); |
1146 slave->ring_position, datagram->state); |
1095 return; |
1147 return; |
1096 } |
1148 } |
1097 |
1149 |
1098 if (datagram->working_counter != 1) { |
1150 if (datagram->working_counter != 1) { |
1099 fsm->state = ec_fsm_coe_error; |
1151 fsm->state = ec_fsm_coe_error; |
1100 EC_ERR("Reception of CoE download response failed on slave %i: ", |
1152 EC_ERR("Reception of CoE download response failed on slave %u: ", |
1101 slave->ring_position); |
1153 slave->ring_position); |
1102 ec_datagram_print_wc_error(datagram); |
1154 ec_datagram_print_wc_error(datagram); |
1103 return; |
1155 return; |
1104 } |
1156 } |
1105 |
1157 |
1113 fsm->state = ec_fsm_coe_error; |
1165 fsm->state = ec_fsm_coe_error; |
1114 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1166 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1115 return; |
1167 return; |
1116 } |
1168 } |
1117 |
1169 |
|
1170 if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { |
|
1171 // check for CoE response again |
|
1172 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1173 fsm->retries = EC_FSM_RETRIES; |
|
1174 fsm->state = ec_fsm_coe_down_check; |
|
1175 return; |
|
1176 } |
|
1177 |
1118 if (rec_size < 6) { |
1178 if (rec_size < 6) { |
1119 fsm->state = ec_fsm_coe_error; |
1179 fsm->state = ec_fsm_coe_error; |
1120 EC_ERR("Received data is too small (%u bytes):\n", rec_size); |
1180 EC_ERR("Received data is too small (%u bytes):\n", rec_size); |
1121 ec_print_data(data, rec_size); |
1181 ec_print_data(data, rec_size); |
1122 return; |
1182 return; |
1123 } |
1183 } |
1124 |
1184 |
1125 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1185 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1126 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1186 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1127 fsm->state = ec_fsm_coe_error; |
1187 fsm->state = ec_fsm_coe_error; |
1128 EC_ERR("Sdo download 0x%04X:%X (%i bytes) aborted on slave %i.\n", |
1188 EC_ERR("Sdo download 0x%04X:%X (%u bytes) aborted on slave %u.\n", |
1129 request->index, request->subindex, request->data_size, |
1189 request->index, request->subindex, request->data_size, |
1130 slave->ring_position); |
1190 slave->ring_position); |
1131 if (rec_size < 10) { |
1191 if (rec_size < 10) { |
1132 EC_ERR("Incomplete Abort command:\n"); |
1192 EC_ERR("Incomplete Abort command:\n"); |
1133 ec_print_data(data, rec_size); |
1193 ec_print_data(data, rec_size); |
1140 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1200 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1141 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1201 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1142 EC_READ_U16(data + 3) != request->index || // index |
1202 EC_READ_U16(data + 3) != request->index || // index |
1143 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1203 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1144 fsm->state = ec_fsm_coe_error; |
1204 fsm->state = ec_fsm_coe_error; |
1145 EC_ERR("Sdo download 0x%04X:%X (%i bytes) failed:\n", |
1205 EC_ERR("Sdo download 0x%04X:%X (%u bytes) failed:\n", |
1146 request->index, request->subindex, request->data_size); |
1206 request->index, request->subindex, request->data_size); |
1147 EC_ERR("Invalid Sdo download response at slave %i!\n", |
1207 EC_ERR("Invalid Sdo download response at slave %u!\n", |
1148 slave->ring_position); |
1208 slave->ring_position); |
1149 ec_print_data(data, rec_size); |
1209 ec_print_data(data, rec_size); |
1150 return; |
1210 return; |
1151 } |
1211 } |
1152 |
1212 |
1212 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1272 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1213 return; // FIXME: check for response first? |
1273 return; // FIXME: check for response first? |
1214 |
1274 |
1215 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1275 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1216 fsm->state = ec_fsm_coe_error; |
1276 fsm->state = ec_fsm_coe_error; |
1217 EC_ERR("Failed to receive CoE upload request for slave %i" |
1277 EC_ERR("Failed to receive CoE upload request for slave %u" |
1218 " (datagram state %i).\n", |
1278 " (datagram state %i).\n", |
1219 slave->ring_position, datagram->state); |
1279 slave->ring_position, datagram->state); |
1220 return; |
1280 return; |
1221 } |
1281 } |
1222 |
1282 |
1223 if (datagram->working_counter != 1) { |
1283 if (datagram->working_counter != 1) { |
1224 fsm->state = ec_fsm_coe_error; |
1284 fsm->state = ec_fsm_coe_error; |
1225 EC_ERR("Reception of CoE upload request failed on slave %i: ", |
1285 EC_ERR("Reception of CoE upload request failed on slave %u: ", |
1226 slave->ring_position); |
1286 slave->ring_position); |
1227 ec_datagram_print_wc_error(datagram); |
1287 ec_datagram_print_wc_error(datagram); |
1228 return; |
1288 return; |
1229 } |
1289 } |
1230 |
1290 |
1249 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1309 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1250 return; |
1310 return; |
1251 |
1311 |
1252 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1312 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1253 fsm->state = ec_fsm_coe_error; |
1313 fsm->state = ec_fsm_coe_error; |
1254 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
1314 EC_ERR("Failed to receive CoE mailbox check datagram from slave %u" |
1255 " (datagram state %i).\n", |
1315 " (datagram state %i).\n", |
1256 slave->ring_position, datagram->state); |
1316 slave->ring_position, datagram->state); |
1257 return; |
1317 return; |
1258 } |
1318 } |
1259 |
1319 |
1260 if (datagram->working_counter != 1) { |
1320 if (datagram->working_counter != 1) { |
1261 fsm->state = ec_fsm_coe_error; |
1321 fsm->state = ec_fsm_coe_error; |
1262 EC_ERR("Reception of CoE mailbox check" |
1322 EC_ERR("Reception of CoE mailbox check" |
1263 " datagram failed on slave %i: ", slave->ring_position); |
1323 " datagram failed on slave %u: ", slave->ring_position); |
1264 ec_datagram_print_wc_error(datagram); |
1324 ec_datagram_print_wc_error(datagram); |
1265 return; |
1325 return; |
1266 } |
1326 } |
1267 |
1327 |
1268 if (!ec_slave_mbox_check(datagram)) { |
1328 if (!ec_slave_mbox_check(datagram)) { |
1269 if (datagram->cycles_received |
1329 if (datagram->cycles_received |
1270 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1330 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1271 fsm->state = ec_fsm_coe_error; |
1331 fsm->state = ec_fsm_coe_error; |
1272 EC_ERR("Timeout while checking Sdo upload on slave %i.\n", |
1332 EC_ERR("Timeout while checking Sdo upload on slave %u.\n", |
1273 slave->ring_position); |
1333 slave->ring_position); |
1274 return; |
1334 return; |
1275 } |
1335 } |
1276 |
1336 |
1277 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1337 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1307 return; // FIXME: request again? |
1367 return; // FIXME: request again? |
1308 |
1368 |
1309 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1369 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1310 fsm->state = ec_fsm_coe_error; |
1370 fsm->state = ec_fsm_coe_error; |
1311 EC_ERR("Failed to receive CoE upload response datagram for" |
1371 EC_ERR("Failed to receive CoE upload response datagram for" |
1312 " slave %i (datagram state %i).\n", |
1372 " slave %u (datagram state %i).\n", |
1313 slave->ring_position, datagram->state); |
1373 slave->ring_position, datagram->state); |
1314 return; |
1374 return; |
1315 } |
1375 } |
1316 |
1376 |
1317 if (datagram->working_counter != 1) { |
1377 if (datagram->working_counter != 1) { |
1318 fsm->state = ec_fsm_coe_error; |
1378 fsm->state = ec_fsm_coe_error; |
1319 EC_ERR("Reception of CoE upload response failed on slave %i: ", |
1379 EC_ERR("Reception of CoE upload response failed on slave %u: ", |
1320 slave->ring_position); |
1380 slave->ring_position); |
1321 ec_datagram_print_wc_error(datagram); |
1381 ec_datagram_print_wc_error(datagram); |
1322 return; |
1382 return; |
1323 } |
1383 } |
1324 |
1384 |
1337 fsm->state = ec_fsm_coe_error; |
1397 fsm->state = ec_fsm_coe_error; |
1338 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1398 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1339 return; |
1399 return; |
1340 } |
1400 } |
1341 |
1401 |
|
1402 if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { |
|
1403 // check for CoE response again |
|
1404 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1405 fsm->retries = EC_FSM_RETRIES; |
|
1406 fsm->state = ec_fsm_coe_up_check; |
|
1407 return; |
|
1408 } |
|
1409 |
1342 if (rec_size < 3) { |
1410 if (rec_size < 3) { |
1343 fsm->state = ec_fsm_coe_error; |
1411 fsm->state = ec_fsm_coe_error; |
1344 EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size); |
1412 EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size); |
1345 ec_print_data(data, rec_size); |
1413 ec_print_data(data, rec_size); |
1346 return; |
1414 return; |
1347 } |
1415 } |
1348 |
1416 |
1349 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1417 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1350 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1418 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1351 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1419 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %u.\n", |
1352 request->index, request->subindex, slave->ring_position); |
1420 request->index, request->subindex, slave->ring_position); |
1353 if (rec_size >= 10) |
1421 if (rec_size >= 10) |
1354 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1422 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1355 else |
1423 else |
1356 EC_ERR("No abort message.\n"); |
1424 EC_ERR("No abort message.\n"); |
1375 EC_READ_U16(data + 3) != request->index || // index |
1443 EC_READ_U16(data + 3) != request->index || // index |
1376 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1444 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1377 fsm->state = ec_fsm_coe_error; |
1445 fsm->state = ec_fsm_coe_error; |
1378 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1446 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1379 request->index, request->subindex); |
1447 request->index, request->subindex); |
1380 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1448 EC_ERR("Invalid Sdo upload expedited response at slave %u!\n", |
1381 slave->ring_position); |
1449 slave->ring_position); |
1382 ec_print_data(data, rec_size); |
1450 ec_print_data(data, rec_size); |
1383 return; |
1451 return; |
1384 } |
1452 } |
1385 |
1453 |
1416 EC_READ_U16(data + 3) != request->index || // index |
1484 EC_READ_U16(data + 3) != request->index || // index |
1417 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1485 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1418 fsm->state = ec_fsm_coe_error; |
1486 fsm->state = ec_fsm_coe_error; |
1419 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1487 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1420 request->index, request->subindex); |
1488 request->index, request->subindex); |
1421 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1489 EC_ERR("Invalid Sdo normal upload response at slave %u!\n", |
1422 slave->ring_position); |
1490 slave->ring_position); |
1423 ec_print_data(data, rec_size); |
1491 ec_print_data(data, rec_size); |
1424 return; |
1492 return; |
1425 } |
1493 } |
1426 |
1494 |
1495 return; // FIXME: check for response first? |
1563 return; // FIXME: check for response first? |
1496 |
1564 |
1497 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1565 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1498 fsm->state = ec_fsm_coe_error; |
1566 fsm->state = ec_fsm_coe_error; |
1499 EC_ERR("Failed to receive CoE upload segment request datagram for" |
1567 EC_ERR("Failed to receive CoE upload segment request datagram for" |
1500 " slave %i (datagram state %i).\n", |
1568 " slave %u (datagram state %i).\n", |
1501 slave->ring_position, datagram->state); |
1569 slave->ring_position, datagram->state); |
1502 return; |
1570 return; |
1503 } |
1571 } |
1504 |
1572 |
1505 if (datagram->working_counter != 1) { |
1573 if (datagram->working_counter != 1) { |
1506 fsm->state = ec_fsm_coe_error; |
1574 fsm->state = ec_fsm_coe_error; |
1507 EC_ERR("Reception of CoE upload segment" |
1575 EC_ERR("Reception of CoE upload segment" |
1508 " request failed on slave %i: ", slave->ring_position); |
1576 " request failed on slave %u: ", slave->ring_position); |
1509 ec_datagram_print_wc_error(datagram); |
1577 ec_datagram_print_wc_error(datagram); |
1510 return; |
1578 return; |
1511 } |
1579 } |
1512 |
1580 |
1513 fsm->cycles_start = datagram->cycles_sent; |
1581 fsm->cycles_start = datagram->cycles_sent; |
1531 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1599 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1532 return; |
1600 return; |
1533 |
1601 |
1534 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1602 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1535 fsm->state = ec_fsm_coe_error; |
1603 fsm->state = ec_fsm_coe_error; |
1536 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1604 EC_ERR("Failed to receive CoE mailbox check datagram for slave %u" |
1537 " (datagram state %i).\n", |
1605 " (datagram state %i).\n", |
1538 slave->ring_position, datagram->state); |
1606 slave->ring_position, datagram->state); |
1539 return; |
1607 return; |
1540 } |
1608 } |
1541 |
1609 |
1542 if (datagram->working_counter != 1) { |
1610 if (datagram->working_counter != 1) { |
1543 fsm->state = ec_fsm_coe_error; |
1611 fsm->state = ec_fsm_coe_error; |
1544 EC_ERR("Reception of CoE mailbox check" |
1612 EC_ERR("Reception of CoE mailbox check" |
1545 " datagram failed on slave %i: ", slave->ring_position); |
1613 " datagram failed on slave %u: ", slave->ring_position); |
1546 ec_datagram_print_wc_error(datagram); |
1614 ec_datagram_print_wc_error(datagram); |
1547 return; |
1615 return; |
1548 } |
1616 } |
1549 |
1617 |
1550 if (!ec_slave_mbox_check(datagram)) { |
1618 if (!ec_slave_mbox_check(datagram)) { |
1551 if (datagram->cycles_received |
1619 if (datagram->cycles_received |
1552 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1620 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1553 fsm->state = ec_fsm_coe_error; |
1621 fsm->state = ec_fsm_coe_error; |
1554 EC_ERR("Timeout while checking Sdo upload segment on slave %i.\n", |
1622 EC_ERR("Timeout while checking Sdo upload segment on slave %u.\n", |
1555 slave->ring_position); |
1623 slave->ring_position); |
1556 return; |
1624 return; |
1557 } |
1625 } |
1558 |
1626 |
1559 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1627 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1590 return; // FIXME: request again? |
1658 return; // FIXME: request again? |
1591 |
1659 |
1592 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1660 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1593 fsm->state = ec_fsm_coe_error; |
1661 fsm->state = ec_fsm_coe_error; |
1594 EC_ERR("Failed to receive CoE upload segment response datagram for" |
1662 EC_ERR("Failed to receive CoE upload segment response datagram for" |
1595 " slave %i (datagram state %i).\n", |
1663 " slave %u (datagram state %i).\n", |
1596 slave->ring_position, datagram->state); |
1664 slave->ring_position, datagram->state); |
1597 return; |
1665 return; |
1598 } |
1666 } |
1599 |
1667 |
1600 if (datagram->working_counter != 1) { |
1668 if (datagram->working_counter != 1) { |
1601 fsm->state = ec_fsm_coe_error; |
1669 fsm->state = ec_fsm_coe_error; |
1602 EC_ERR("Reception of CoE upload segment" |
1670 EC_ERR("Reception of CoE upload segment" |
1603 " response failed on slave %i: ", slave->ring_position); |
1671 " response failed on slave %u: ", slave->ring_position); |
1604 ec_datagram_print_wc_error(datagram); |
1672 ec_datagram_print_wc_error(datagram); |
1605 return; |
1673 return; |
1606 } |
1674 } |
1607 |
1675 |
1608 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1676 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1617 } |
1685 } |
1618 |
1686 |
1619 if (mbox_prot != 0x03) { // CoE |
1687 if (mbox_prot != 0x03) { // CoE |
1620 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1688 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1621 fsm->state = ec_fsm_coe_error; |
1689 fsm->state = ec_fsm_coe_error; |
1622 return; |
1690 return; |
|
1691 } |
|
1692 |
|
1693 if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) { |
|
1694 // check for CoE response again |
|
1695 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1696 fsm->retries = EC_FSM_RETRIES; |
|
1697 fsm->state = ec_fsm_coe_up_seg_check; |
|
1698 return; |
1623 } |
1699 } |
1624 |
1700 |
1625 if (rec_size < 10) { |
1701 if (rec_size < 10) { |
1626 EC_ERR("Received currupted Sdo upload segment response!\n"); |
1702 EC_ERR("Received currupted Sdo upload segment response!\n"); |
1627 ec_print_data(data, rec_size); |
1703 ec_print_data(data, rec_size); |
1628 fsm->state = ec_fsm_coe_error; |
1704 fsm->state = ec_fsm_coe_error; |
1629 return; |
1705 return; |
1630 } |
1706 } |
1631 |
1707 |
1632 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1708 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1633 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1709 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1634 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1710 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %u.\n", |
1635 request->index, request->subindex, slave->ring_position); |
1711 request->index, request->subindex, slave->ring_position); |
1636 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1712 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1637 fsm->state = ec_fsm_coe_error; |
1713 fsm->state = ec_fsm_coe_error; |
1638 return; |
1714 return; |
1639 } |
1715 } |
1640 |
1716 |
1641 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1717 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1642 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1718 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1643 EC_ERR("Sdo upload 0x%04X:%X failed:\n", request->index, request->subindex); |
1719 EC_ERR("Sdo upload 0x%04X:%X failed:\n", request->index, request->subindex); |
1644 EC_ERR("Invalid Sdo upload segment response at slave %i!\n", |
1720 EC_ERR("Invalid Sdo upload segment response at slave %u!\n", |
1645 slave->ring_position); |
1721 slave->ring_position); |
1646 ec_print_data(data, rec_size); |
1722 ec_print_data(data, rec_size); |
1647 fsm->state = ec_fsm_coe_error; |
1723 fsm->state = ec_fsm_coe_error; |
1648 return; |
1724 return; |
1649 } |
1725 } |
1650 |
1726 |
1651 last_segment = EC_READ_U8(data + 2) & 0x01; |
1727 last_segment = EC_READ_U8(data + 2) & 0x01; |
1652 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1728 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1653 data_size = rec_size - 10; |
1729 data_size = rec_size - 10; |