258 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
258 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
259 EC_WRITE_U8 (data + 3, 0x00); |
259 EC_WRITE_U8 (data + 3, 0x00); |
260 EC_WRITE_U16(data + 4, 0x0000); |
260 EC_WRITE_U16(data + 4, 0x0000); |
261 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
261 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
262 |
262 |
263 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
264 fsm->retries = EC_FSM_RETRIES; |
263 fsm->retries = EC_FSM_RETRIES; |
265 fsm->state = ec_fsm_coe_dict_request; |
264 fsm->state = ec_fsm_coe_dict_request; |
266 } |
265 } |
267 |
266 |
268 /*****************************************************************************/ |
267 /*****************************************************************************/ |
275 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
274 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
276 { |
275 { |
277 ec_datagram_t *datagram = fsm->datagram; |
276 ec_datagram_t *datagram = fsm->datagram; |
278 ec_slave_t *slave = fsm->slave; |
277 ec_slave_t *slave = fsm->slave; |
279 |
278 |
280 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
279 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
281 // FIXME: request again? |
280 return; // FIXME: request again? |
282 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
283 return; |
|
284 } |
|
285 |
281 |
286 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
282 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
287 fsm->state = ec_fsm_coe_error; |
283 fsm->state = ec_fsm_coe_error; |
288 EC_ERR("Failed to receive CoE dictionary request datagram for" |
284 EC_ERR("Failed to receive CoE dictionary request datagram for" |
289 " slave %i.\n", slave->ring_position); |
285 " slave %i (datagram state %i).\n", |
|
286 slave->ring_position, datagram->state); |
290 return; |
287 return; |
291 } |
288 } |
292 |
289 |
293 if (datagram->working_counter != 1) { |
290 if (datagram->working_counter != 1) { |
294 fsm->state = ec_fsm_coe_error; |
291 fsm->state = ec_fsm_coe_error; |
298 } |
295 } |
299 |
296 |
300 fsm->cycles_start = datagram->cycles_sent; |
297 fsm->cycles_start = datagram->cycles_sent; |
301 |
298 |
302 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
299 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
303 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
304 fsm->retries = EC_FSM_RETRIES; |
300 fsm->retries = EC_FSM_RETRIES; |
305 fsm->state = ec_fsm_coe_dict_check; |
301 fsm->state = ec_fsm_coe_dict_check; |
306 } |
302 } |
307 |
303 |
308 /*****************************************************************************/ |
304 /*****************************************************************************/ |
314 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
310 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
315 { |
311 { |
316 ec_datagram_t *datagram = fsm->datagram; |
312 ec_datagram_t *datagram = fsm->datagram; |
317 ec_slave_t *slave = fsm->slave; |
313 ec_slave_t *slave = fsm->slave; |
318 |
314 |
319 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
315 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
320 ec_master_queue_datagram(fsm->slave->master, datagram); |
316 return; |
321 return; |
317 |
322 } |
318 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
323 |
319 fsm->state = ec_fsm_coe_error; |
324 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
320 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
325 fsm->state = ec_fsm_coe_error; |
321 " (datagram state %i).\n", |
326 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n", |
322 slave->ring_position, datagram->state); |
327 slave->ring_position); |
|
328 return; |
323 return; |
329 } |
324 } |
330 |
325 |
331 if (datagram->working_counter != 1) { |
326 if (datagram->working_counter != 1) { |
332 fsm->state = ec_fsm_coe_error; |
327 fsm->state = ec_fsm_coe_error; |
344 slave->ring_position); |
339 slave->ring_position); |
345 return; |
340 return; |
346 } |
341 } |
347 |
342 |
348 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
343 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
349 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
350 fsm->retries = EC_FSM_RETRIES; |
344 fsm->retries = EC_FSM_RETRIES; |
351 return; |
345 return; |
352 } |
346 } |
353 |
347 |
354 // Fetch response |
348 // Fetch response |
355 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
349 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
356 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
357 fsm->retries = EC_FSM_RETRIES; |
350 fsm->retries = EC_FSM_RETRIES; |
358 fsm->state = ec_fsm_coe_dict_response; |
351 fsm->state = ec_fsm_coe_dict_response; |
359 } |
352 } |
360 |
353 |
361 /*****************************************************************************/ |
354 /*****************************************************************************/ |
373 size_t rec_size; |
366 size_t rec_size; |
374 unsigned int sdo_count, i; |
367 unsigned int sdo_count, i; |
375 uint16_t sdo_index; |
368 uint16_t sdo_index; |
376 ec_sdo_t *sdo; |
369 ec_sdo_t *sdo; |
377 |
370 |
378 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
371 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
379 // FIXME: request again? |
372 return; // FIXME: request again? |
380 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
381 return; |
|
382 } |
|
383 |
373 |
384 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
374 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
385 fsm->state = ec_fsm_coe_error; |
375 fsm->state = ec_fsm_coe_error; |
386 EC_ERR("Failed to receive CoE dictionary response datagram for" |
376 EC_ERR("Failed to receive CoE dictionary response datagram for" |
387 " slave %i.\n", slave->ring_position); |
377 " slave %i (datagram state %i).\n", |
|
378 slave->ring_position, datagram->state); |
388 return; |
379 return; |
389 } |
380 } |
390 |
381 |
391 if (datagram->working_counter != 1) { |
382 if (datagram->working_counter != 1) { |
392 fsm->state = ec_fsm_coe_error; |
383 fsm->state = ec_fsm_coe_error; |
459 } |
450 } |
460 |
451 |
461 if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again. |
452 if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again. |
462 fsm->cycles_start = datagram->cycles_sent; |
453 fsm->cycles_start = datagram->cycles_sent; |
463 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
454 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
464 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
465 fsm->retries = EC_FSM_RETRIES; |
455 fsm->retries = EC_FSM_RETRIES; |
466 fsm->state = ec_fsm_coe_dict_check; |
456 fsm->state = ec_fsm_coe_dict_check; |
467 return; |
457 return; |
468 } |
458 } |
469 |
459 |
485 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
475 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
486 EC_WRITE_U8 (data + 3, 0x00); |
476 EC_WRITE_U8 (data + 3, 0x00); |
487 EC_WRITE_U16(data + 4, 0x0000); |
477 EC_WRITE_U16(data + 4, 0x0000); |
488 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
478 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
489 |
479 |
490 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
491 fsm->retries = EC_FSM_RETRIES; |
480 fsm->retries = EC_FSM_RETRIES; |
492 fsm->state = ec_fsm_coe_dict_desc_request; |
481 fsm->state = ec_fsm_coe_dict_desc_request; |
493 } |
482 } |
494 |
483 |
495 /*****************************************************************************/ |
484 /*****************************************************************************/ |
502 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
491 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
503 { |
492 { |
504 ec_datagram_t *datagram = fsm->datagram; |
493 ec_datagram_t *datagram = fsm->datagram; |
505 ec_slave_t *slave = fsm->slave; |
494 ec_slave_t *slave = fsm->slave; |
506 |
495 |
507 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
496 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
508 // FIXME: check for response first? |
497 return; // FIXME: check for response first? |
509 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
510 return; |
|
511 } |
|
512 |
498 |
513 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
499 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
514 fsm->state = ec_fsm_coe_error; |
500 fsm->state = ec_fsm_coe_error; |
515 EC_ERR("Failed to receive CoE SDO description request datagram for" |
501 EC_ERR("Failed to receive CoE SDO description request datagram for" |
516 " slave %i.\n", slave->ring_position); |
502 " slave %i (datagram state %i).\n", |
|
503 slave->ring_position, datagram->state); |
517 return; |
504 return; |
518 } |
505 } |
519 |
506 |
520 if (datagram->working_counter != 1) { |
507 if (datagram->working_counter != 1) { |
521 fsm->state = ec_fsm_coe_error; |
508 fsm->state = ec_fsm_coe_error; |
525 } |
512 } |
526 |
513 |
527 fsm->cycles_start = datagram->cycles_sent; |
514 fsm->cycles_start = datagram->cycles_sent; |
528 |
515 |
529 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
516 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
530 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
531 fsm->retries = EC_FSM_RETRIES; |
517 fsm->retries = EC_FSM_RETRIES; |
532 fsm->state = ec_fsm_coe_dict_desc_check; |
518 fsm->state = ec_fsm_coe_dict_desc_check; |
533 } |
519 } |
534 |
520 |
535 /*****************************************************************************/ |
521 /*****************************************************************************/ |
541 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
527 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
542 { |
528 { |
543 ec_datagram_t *datagram = fsm->datagram; |
529 ec_datagram_t *datagram = fsm->datagram; |
544 ec_slave_t *slave = fsm->slave; |
530 ec_slave_t *slave = fsm->slave; |
545 |
531 |
546 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
532 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
547 ec_master_queue_datagram(fsm->slave->master, datagram); |
533 return; |
548 return; |
534 |
549 } |
535 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
550 |
536 fsm->state = ec_fsm_coe_error; |
551 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
537 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
552 fsm->state = ec_fsm_coe_error; |
538 " (datagram state %i).\n", |
553 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n", |
539 slave->ring_position, datagram->state); |
554 slave->ring_position); |
|
555 return; |
540 return; |
556 } |
541 } |
557 |
542 |
558 if (datagram->working_counter != 1) { |
543 if (datagram->working_counter != 1) { |
559 fsm->state = ec_fsm_coe_error; |
544 fsm->state = ec_fsm_coe_error; |
570 slave->ring_position); |
555 slave->ring_position); |
571 return; |
556 return; |
572 } |
557 } |
573 |
558 |
574 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
559 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
575 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
576 fsm->retries = EC_FSM_RETRIES; |
560 fsm->retries = EC_FSM_RETRIES; |
577 return; |
561 return; |
578 } |
562 } |
579 |
563 |
580 // Fetch response |
564 // Fetch response |
581 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
565 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
582 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
583 fsm->retries = EC_FSM_RETRIES; |
566 fsm->retries = EC_FSM_RETRIES; |
584 fsm->state = ec_fsm_coe_dict_desc_response; |
567 fsm->state = ec_fsm_coe_dict_desc_response; |
585 } |
568 } |
586 |
569 |
587 /*****************************************************************************/ |
570 /*****************************************************************************/ |
598 ec_slave_t *slave = fsm->slave; |
581 ec_slave_t *slave = fsm->slave; |
599 ec_sdo_t *sdo = fsm->sdo; |
582 ec_sdo_t *sdo = fsm->sdo; |
600 uint8_t *data, mbox_prot; |
583 uint8_t *data, mbox_prot; |
601 size_t rec_size, name_size; |
584 size_t rec_size, name_size; |
602 |
585 |
603 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
586 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
604 // FIXME: request again? |
587 return; // FIXME: request again? |
605 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
606 return; |
|
607 } |
|
608 |
588 |
609 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
589 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
610 fsm->state = ec_fsm_coe_error; |
590 fsm->state = ec_fsm_coe_error; |
611 EC_ERR("Failed to receive CoE SDO description response datagram from" |
591 EC_ERR("Failed to receive CoE SDO description response datagram from" |
612 " slave %i.\n", slave->ring_position); |
592 " slave %i (datagram state %i).\n", |
|
593 slave->ring_position, datagram->state); |
613 return; |
594 return; |
614 } |
595 } |
615 |
596 |
616 if (datagram->working_counter != 1) { |
597 if (datagram->working_counter != 1) { |
617 fsm->state = ec_fsm_coe_error; |
598 fsm->state = ec_fsm_coe_error; |
696 EC_WRITE_U16(data + 4, 0x0000); |
677 EC_WRITE_U16(data + 4, 0x0000); |
697 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
678 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
698 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
679 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
699 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
680 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
700 |
681 |
701 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
702 fsm->retries = EC_FSM_RETRIES; |
682 fsm->retries = EC_FSM_RETRIES; |
703 fsm->state = ec_fsm_coe_dict_entry_request; |
683 fsm->state = ec_fsm_coe_dict_entry_request; |
704 } |
684 } |
705 |
685 |
706 /*****************************************************************************/ |
686 /*****************************************************************************/ |
714 /**< finite state machine */) |
694 /**< finite state machine */) |
715 { |
695 { |
716 ec_datagram_t *datagram = fsm->datagram; |
696 ec_datagram_t *datagram = fsm->datagram; |
717 ec_slave_t *slave = fsm->slave; |
697 ec_slave_t *slave = fsm->slave; |
718 |
698 |
719 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
699 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
720 // FIXME: check for response first? |
700 return; // FIXME: check for response first? |
721 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
722 return; |
|
723 } |
|
724 |
701 |
725 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
702 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
726 fsm->state = ec_fsm_coe_error; |
703 fsm->state = ec_fsm_coe_error; |
727 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
704 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
728 " slave %i.\n", slave->ring_position); |
705 " slave %i (datagram state %i).\n", |
|
706 slave->ring_position, datagram->state); |
729 return; |
707 return; |
730 } |
708 } |
731 |
709 |
732 if (datagram->working_counter != 1) { |
710 if (datagram->working_counter != 1) { |
733 fsm->state = ec_fsm_coe_error; |
711 fsm->state = ec_fsm_coe_error; |
737 } |
715 } |
738 |
716 |
739 fsm->cycles_start = datagram->cycles_sent; |
717 fsm->cycles_start = datagram->cycles_sent; |
740 |
718 |
741 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
719 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
742 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
743 fsm->retries = EC_FSM_RETRIES; |
720 fsm->retries = EC_FSM_RETRIES; |
744 fsm->state = ec_fsm_coe_dict_entry_check; |
721 fsm->state = ec_fsm_coe_dict_entry_check; |
745 } |
722 } |
746 |
723 |
747 /*****************************************************************************/ |
724 /*****************************************************************************/ |
754 /**< finite state machine */) |
731 /**< finite state machine */) |
755 { |
732 { |
756 ec_datagram_t *datagram = fsm->datagram; |
733 ec_datagram_t *datagram = fsm->datagram; |
757 ec_slave_t *slave = fsm->slave; |
734 ec_slave_t *slave = fsm->slave; |
758 |
735 |
759 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
736 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
760 ec_master_queue_datagram(fsm->slave->master, datagram); |
737 return; |
761 return; |
738 |
762 } |
739 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
763 |
740 fsm->state = ec_fsm_coe_error; |
764 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
741 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
765 fsm->state = ec_fsm_coe_error; |
742 " (datagram state %i).\n", |
766 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n", |
743 slave->ring_position, datagram->state); |
767 slave->ring_position); |
|
768 return; |
744 return; |
769 } |
745 } |
770 |
746 |
771 if (datagram->working_counter != 1) { |
747 if (datagram->working_counter != 1) { |
772 fsm->state = ec_fsm_coe_error; |
748 fsm->state = ec_fsm_coe_error; |
783 slave->ring_position); |
759 slave->ring_position); |
784 return; |
760 return; |
785 } |
761 } |
786 |
762 |
787 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
763 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
788 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
789 fsm->retries = EC_FSM_RETRIES; |
764 fsm->retries = EC_FSM_RETRIES; |
790 return; |
765 return; |
791 } |
766 } |
792 |
767 |
793 // Fetch response |
768 // Fetch response |
794 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
769 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
795 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
796 fsm->retries = EC_FSM_RETRIES; |
770 fsm->retries = EC_FSM_RETRIES; |
797 fsm->state = ec_fsm_coe_dict_entry_response; |
771 fsm->state = ec_fsm_coe_dict_entry_response; |
798 } |
772 } |
799 |
773 |
800 /*****************************************************************************/ |
774 /*****************************************************************************/ |
812 ec_sdo_t *sdo = fsm->sdo; |
786 ec_sdo_t *sdo = fsm->sdo; |
813 uint8_t *data, mbox_prot; |
787 uint8_t *data, mbox_prot; |
814 size_t rec_size, data_size; |
788 size_t rec_size, data_size; |
815 ec_sdo_entry_t *entry; |
789 ec_sdo_entry_t *entry; |
816 |
790 |
817 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
791 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
818 // FIXME: request again? |
792 return; // FIXME: request again? |
819 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
820 return; |
|
821 } |
|
822 |
793 |
823 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
794 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
824 fsm->state = ec_fsm_coe_error; |
795 fsm->state = ec_fsm_coe_error; |
825 EC_ERR("Failed to receive CoE SDO description response datagram from" |
796 EC_ERR("Failed to receive CoE SDO description response datagram from" |
826 " slave %i.\n", slave->ring_position); |
797 " slave %i (datagram state %i).\n", |
|
798 slave->ring_position, datagram->state); |
827 return; |
799 return; |
828 } |
800 } |
829 |
801 |
830 if (datagram->working_counter != 1) { |
802 if (datagram->working_counter != 1) { |
831 fsm->state = ec_fsm_coe_error; |
803 fsm->state = ec_fsm_coe_error; |
921 EC_WRITE_U16(data + 4, 0x0000); |
893 EC_WRITE_U16(data + 4, 0x0000); |
922 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
894 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
923 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
895 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
924 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
896 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
925 |
897 |
926 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
927 fsm->retries = EC_FSM_RETRIES; |
898 fsm->retries = EC_FSM_RETRIES; |
928 fsm->state = ec_fsm_coe_dict_entry_request; |
899 fsm->state = ec_fsm_coe_dict_entry_request; |
929 return; |
900 return; |
930 } |
901 } |
931 |
902 |
942 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
913 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
943 EC_WRITE_U8 (data + 3, 0x00); |
914 EC_WRITE_U8 (data + 3, 0x00); |
944 EC_WRITE_U16(data + 4, 0x0000); |
915 EC_WRITE_U16(data + 4, 0x0000); |
945 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
916 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
946 |
917 |
947 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
948 fsm->retries = EC_FSM_RETRIES; |
918 fsm->retries = EC_FSM_RETRIES; |
949 fsm->state = ec_fsm_coe_dict_desc_request; |
919 fsm->state = ec_fsm_coe_dict_desc_request; |
950 return; |
920 return; |
951 } |
921 } |
952 |
922 |
990 EC_WRITE_U16(data + 3, sdodata->index); |
960 EC_WRITE_U16(data + 3, sdodata->index); |
991 EC_WRITE_U8 (data + 5, sdodata->subindex); |
961 EC_WRITE_U8 (data + 5, sdodata->subindex); |
992 EC_WRITE_U32(data + 6, sdodata->size); |
962 EC_WRITE_U32(data + 6, sdodata->size); |
993 memcpy(data + 10, sdodata->data, sdodata->size); |
963 memcpy(data + 10, sdodata->data, sdodata->size); |
994 |
964 |
995 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
996 fsm->retries = EC_FSM_RETRIES; |
965 fsm->retries = EC_FSM_RETRIES; |
997 fsm->state = ec_fsm_coe_down_request; |
966 fsm->state = ec_fsm_coe_down_request; |
998 } |
967 } |
999 |
968 |
1000 /*****************************************************************************/ |
969 /*****************************************************************************/ |
1007 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
976 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1008 { |
977 { |
1009 ec_datagram_t *datagram = fsm->datagram; |
978 ec_datagram_t *datagram = fsm->datagram; |
1010 ec_slave_t *slave = fsm->slave; |
979 ec_slave_t *slave = fsm->slave; |
1011 |
980 |
1012 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
981 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1013 // FIXME: check for response first? |
982 return; // FIXME: check for response first? |
1014 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1015 return; |
|
1016 } |
|
1017 |
983 |
1018 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
984 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1019 fsm->state = ec_fsm_coe_error; |
985 fsm->state = ec_fsm_coe_error; |
1020 EC_ERR("Failed to receive CoE download request datagram for" |
986 EC_ERR("Failed to receive CoE download request datagram for" |
1021 " slave %i.\n", slave->ring_position); |
987 " slave %i (datagram state %i).\n", |
|
988 slave->ring_position, datagram->state); |
1022 return; |
989 return; |
1023 } |
990 } |
1024 |
991 |
1025 if (datagram->working_counter != 1) { |
992 if (datagram->working_counter != 1) { |
1026 fsm->state = ec_fsm_coe_error; |
993 fsm->state = ec_fsm_coe_error; |
1030 } |
997 } |
1031 |
998 |
1032 fsm->cycles_start = datagram->cycles_sent; |
999 fsm->cycles_start = datagram->cycles_sent; |
1033 |
1000 |
1034 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1001 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1035 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1036 fsm->retries = EC_FSM_RETRIES; |
1002 fsm->retries = EC_FSM_RETRIES; |
1037 fsm->state = ec_fsm_coe_down_check; |
1003 fsm->state = ec_fsm_coe_down_check; |
1038 } |
1004 } |
1039 |
1005 |
1040 /*****************************************************************************/ |
1006 /*****************************************************************************/ |
1046 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1012 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1047 { |
1013 { |
1048 ec_datagram_t *datagram = fsm->datagram; |
1014 ec_datagram_t *datagram = fsm->datagram; |
1049 ec_slave_t *slave = fsm->slave; |
1015 ec_slave_t *slave = fsm->slave; |
1050 |
1016 |
1051 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1017 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1052 ec_master_queue_datagram(fsm->slave->master, datagram); |
1018 return; |
1053 return; |
1019 |
1054 } |
1020 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1055 |
1021 fsm->state = ec_fsm_coe_error; |
1056 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1022 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1057 fsm->state = ec_fsm_coe_error; |
1023 " (datagram state %i).\n", |
1058 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n", |
1024 slave->ring_position, datagram->state); |
1059 slave->ring_position); |
|
1060 return; |
1025 return; |
1061 } |
1026 } |
1062 |
1027 |
1063 if (datagram->working_counter != 1) { |
1028 if (datagram->working_counter != 1) { |
1064 fsm->state = ec_fsm_coe_error; |
1029 fsm->state = ec_fsm_coe_error; |
1075 slave->ring_position); |
1040 slave->ring_position); |
1076 return; |
1041 return; |
1077 } |
1042 } |
1078 |
1043 |
1079 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1044 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1080 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1081 fsm->retries = EC_FSM_RETRIES; |
1045 fsm->retries = EC_FSM_RETRIES; |
1082 return; |
1046 return; |
1083 } |
1047 } |
1084 |
1048 |
1085 // Fetch response |
1049 // Fetch response |
1086 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1050 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1087 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1088 fsm->retries = EC_FSM_RETRIES; |
1051 fsm->retries = EC_FSM_RETRIES; |
1089 fsm->state = ec_fsm_coe_down_response; |
1052 fsm->state = ec_fsm_coe_down_response; |
1090 } |
1053 } |
1091 |
1054 |
1092 /*****************************************************************************/ |
1055 /*****************************************************************************/ |
1102 ec_slave_t *slave = fsm->slave; |
1065 ec_slave_t *slave = fsm->slave; |
1103 uint8_t *data, mbox_prot; |
1066 uint8_t *data, mbox_prot; |
1104 size_t rec_size; |
1067 size_t rec_size; |
1105 ec_sdo_data_t *sdodata = fsm->sdodata; |
1068 ec_sdo_data_t *sdodata = fsm->sdodata; |
1106 |
1069 |
1107 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1070 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1108 // FIXME: request again? |
1071 return; // FIXME: request again? |
1109 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1110 return; |
|
1111 } |
|
1112 |
1072 |
1113 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1073 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1114 fsm->state = ec_fsm_coe_error; |
1074 fsm->state = ec_fsm_coe_error; |
1115 EC_ERR("Failed to receive CoE download response datagram from" |
1075 EC_ERR("Failed to receive CoE download response datagram from" |
1116 " slave %i.\n", slave->ring_position); |
1076 " slave %i (datagram state %i).\n", |
|
1077 slave->ring_position, datagram->state); |
1117 return; |
1078 return; |
1118 } |
1079 } |
1119 |
1080 |
1120 if (datagram->working_counter != 1) { |
1081 if (datagram->working_counter != 1) { |
1121 fsm->state = ec_fsm_coe_error; |
1082 fsm->state = ec_fsm_coe_error; |
1208 if (master->debug_level) { |
1169 if (master->debug_level) { |
1209 EC_DBG("Upload request:\n"); |
1170 EC_DBG("Upload request:\n"); |
1210 ec_print_data(data, 10); |
1171 ec_print_data(data, 10); |
1211 } |
1172 } |
1212 |
1173 |
1213 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1214 fsm->retries = EC_FSM_RETRIES; |
1174 fsm->retries = EC_FSM_RETRIES; |
1215 fsm->state = ec_fsm_coe_up_request; |
1175 fsm->state = ec_fsm_coe_up_request; |
1216 } |
1176 } |
1217 |
1177 |
1218 /*****************************************************************************/ |
1178 /*****************************************************************************/ |
1225 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1185 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1226 { |
1186 { |
1227 ec_datagram_t *datagram = fsm->datagram; |
1187 ec_datagram_t *datagram = fsm->datagram; |
1228 ec_slave_t *slave = fsm->slave; |
1188 ec_slave_t *slave = fsm->slave; |
1229 |
1189 |
1230 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1190 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1231 // FIXME: check for response first? |
1191 return; // FIXME: check for response first? |
1232 ec_master_queue_datagram(fsm->slave->master, datagram); |
1192 |
1233 return; |
1193 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1234 } |
1194 fsm->state = ec_fsm_coe_error; |
1235 |
1195 EC_ERR("Failed to receive CoE upload request for slave %i" |
1236 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1196 " (datagram state %i).\n", |
1237 fsm->state = ec_fsm_coe_error; |
1197 slave->ring_position, datagram->state); |
1238 EC_ERR("Failed to receive CoE upload request for slave %i.\n", |
|
1239 slave->ring_position); |
|
1240 return; |
1198 return; |
1241 } |
1199 } |
1242 |
1200 |
1243 if (datagram->working_counter != 1) { |
1201 if (datagram->working_counter != 1) { |
1244 fsm->state = ec_fsm_coe_error; |
1202 fsm->state = ec_fsm_coe_error; |
1248 } |
1206 } |
1249 |
1207 |
1250 fsm->cycles_start = datagram->cycles_sent; |
1208 fsm->cycles_start = datagram->cycles_sent; |
1251 |
1209 |
1252 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1210 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1253 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1254 fsm->retries = EC_FSM_RETRIES; |
1211 fsm->retries = EC_FSM_RETRIES; |
1255 fsm->state = ec_fsm_coe_up_check; |
1212 fsm->state = ec_fsm_coe_up_check; |
1256 } |
1213 } |
1257 |
1214 |
1258 /*****************************************************************************/ |
1215 /*****************************************************************************/ |
1264 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1221 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1265 { |
1222 { |
1266 ec_datagram_t *datagram = fsm->datagram; |
1223 ec_datagram_t *datagram = fsm->datagram; |
1267 ec_slave_t *slave = fsm->slave; |
1224 ec_slave_t *slave = fsm->slave; |
1268 |
1225 |
1269 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1226 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1270 ec_master_queue_datagram(fsm->slave->master, datagram); |
1227 return; |
1271 return; |
1228 |
1272 } |
1229 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1273 |
1230 fsm->state = ec_fsm_coe_error; |
1274 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1231 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
1275 fsm->state = ec_fsm_coe_error; |
1232 " (datagram state %i).\n", |
1276 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n", |
1233 slave->ring_position, datagram->state); |
1277 slave->ring_position); |
|
1278 return; |
1234 return; |
1279 } |
1235 } |
1280 |
1236 |
1281 if (datagram->working_counter != 1) { |
1237 if (datagram->working_counter != 1) { |
1282 fsm->state = ec_fsm_coe_error; |
1238 fsm->state = ec_fsm_coe_error; |
1293 slave->ring_position); |
1249 slave->ring_position); |
1294 return; |
1250 return; |
1295 } |
1251 } |
1296 |
1252 |
1297 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1253 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1298 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1299 fsm->retries = EC_FSM_RETRIES; |
1254 fsm->retries = EC_FSM_RETRIES; |
1300 return; |
1255 return; |
1301 } |
1256 } |
1302 |
1257 |
1303 // Fetch response |
1258 // Fetch response |
1304 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1259 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1305 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1306 fsm->retries = EC_FSM_RETRIES; |
1260 fsm->retries = EC_FSM_RETRIES; |
1307 fsm->state = ec_fsm_coe_up_response; |
1261 fsm->state = ec_fsm_coe_up_response; |
1308 } |
1262 } |
1309 |
1263 |
1310 /*****************************************************************************/ |
1264 /*****************************************************************************/ |
1325 ec_sdo_t *sdo = request->sdo; |
1279 ec_sdo_t *sdo = request->sdo; |
1326 ec_sdo_entry_t *entry = request->entry; |
1280 ec_sdo_entry_t *entry = request->entry; |
1327 uint32_t complete_size; |
1281 uint32_t complete_size; |
1328 unsigned int expedited, size_specified; |
1282 unsigned int expedited, size_specified; |
1329 |
1283 |
1330 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1284 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1331 // FIXME: request again? |
1285 return; // FIXME: request again? |
1332 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1333 return; |
|
1334 } |
|
1335 |
1286 |
1336 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1287 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1337 fsm->state = ec_fsm_coe_error; |
1288 fsm->state = ec_fsm_coe_error; |
1338 EC_ERR("Failed to receive CoE upload response datagram for" |
1289 EC_ERR("Failed to receive CoE upload response datagram for" |
1339 " slave %i.\n", slave->ring_position); |
1290 " slave %i (datagram state %i).\n", |
|
1291 slave->ring_position, datagram->state); |
1340 return; |
1292 return; |
1341 } |
1293 } |
1342 |
1294 |
1343 if (datagram->working_counter != 1) { |
1295 if (datagram->working_counter != 1) { |
1344 fsm->state = ec_fsm_coe_error; |
1296 fsm->state = ec_fsm_coe_error; |
1451 if (master->debug_level) { |
1403 if (master->debug_level) { |
1452 EC_DBG("Upload segment request:\n"); |
1404 EC_DBG("Upload segment request:\n"); |
1453 ec_print_data(data, 3); |
1405 ec_print_data(data, 3); |
1454 } |
1406 } |
1455 |
1407 |
1456 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1457 fsm->retries = EC_FSM_RETRIES; |
1408 fsm->retries = EC_FSM_RETRIES; |
1458 fsm->state = ec_fsm_coe_up_seg_request; |
1409 fsm->state = ec_fsm_coe_up_seg_request; |
1459 return; |
1410 return; |
1460 } |
1411 } |
1461 } |
1412 } |
1473 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1424 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1474 { |
1425 { |
1475 ec_datagram_t *datagram = fsm->datagram; |
1426 ec_datagram_t *datagram = fsm->datagram; |
1476 ec_slave_t *slave = fsm->slave; |
1427 ec_slave_t *slave = fsm->slave; |
1477 |
1428 |
1478 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1429 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1479 // FIXME: check for response first? |
1430 return; // FIXME: check for response first? |
1480 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1481 return; |
|
1482 } |
|
1483 |
1431 |
1484 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1432 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1485 fsm->state = ec_fsm_coe_error; |
1433 fsm->state = ec_fsm_coe_error; |
1486 EC_ERR("Failed to receive CoE upload segment request datagram for" |
1434 EC_ERR("Failed to receive CoE upload segment request datagram for" |
1487 " slave %i.\n", slave->ring_position); |
1435 " slave %i (datagram state %i).\n", |
|
1436 slave->ring_position, datagram->state); |
1488 return; |
1437 return; |
1489 } |
1438 } |
1490 |
1439 |
1491 if (datagram->working_counter != 1) { |
1440 if (datagram->working_counter != 1) { |
1492 fsm->state = ec_fsm_coe_error; |
1441 fsm->state = ec_fsm_coe_error; |
1496 } |
1445 } |
1497 |
1446 |
1498 fsm->cycles_start = datagram->cycles_sent; |
1447 fsm->cycles_start = datagram->cycles_sent; |
1499 |
1448 |
1500 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1449 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1501 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1502 fsm->retries = EC_FSM_RETRIES; |
1450 fsm->retries = EC_FSM_RETRIES; |
1503 fsm->state = ec_fsm_coe_up_seg_check; |
1451 fsm->state = ec_fsm_coe_up_seg_check; |
1504 } |
1452 } |
1505 |
1453 |
1506 /*****************************************************************************/ |
1454 /*****************************************************************************/ |
1512 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1460 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1513 { |
1461 { |
1514 ec_datagram_t *datagram = fsm->datagram; |
1462 ec_datagram_t *datagram = fsm->datagram; |
1515 ec_slave_t *slave = fsm->slave; |
1463 ec_slave_t *slave = fsm->slave; |
1516 |
1464 |
1517 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1465 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1518 ec_master_queue_datagram(fsm->slave->master, datagram); |
1466 return; |
1519 return; |
1467 |
1520 } |
1468 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1521 |
1469 fsm->state = ec_fsm_coe_error; |
1522 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1470 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1523 fsm->state = ec_fsm_coe_error; |
1471 " (datagram state %i).\n", |
1524 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n", |
1472 slave->ring_position, datagram->state); |
1525 slave->ring_position); |
|
1526 return; |
1473 return; |
1527 } |
1474 } |
1528 |
1475 |
1529 if (datagram->working_counter != 1) { |
1476 if (datagram->working_counter != 1) { |
1530 fsm->state = ec_fsm_coe_error; |
1477 fsm->state = ec_fsm_coe_error; |
1541 slave->ring_position); |
1488 slave->ring_position); |
1542 return; |
1489 return; |
1543 } |
1490 } |
1544 |
1491 |
1545 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1492 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1546 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1547 fsm->retries = EC_FSM_RETRIES; |
1493 fsm->retries = EC_FSM_RETRIES; |
1548 return; |
1494 return; |
1549 } |
1495 } |
1550 |
1496 |
1551 // Fetch response |
1497 // Fetch response |
1552 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1498 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1553 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1554 fsm->retries = EC_FSM_RETRIES; |
1499 fsm->retries = EC_FSM_RETRIES; |
1555 fsm->state = ec_fsm_coe_up_seg_response; |
1500 fsm->state = ec_fsm_coe_up_seg_response; |
1556 } |
1501 } |
1557 |
1502 |
1558 /*****************************************************************************/ |
1503 /*****************************************************************************/ |
1573 ec_sdo_t *sdo = request->sdo; |
1518 ec_sdo_t *sdo = request->sdo; |
1574 ec_sdo_entry_t *entry = request->entry; |
1519 ec_sdo_entry_t *entry = request->entry; |
1575 uint32_t seg_size; |
1520 uint32_t seg_size; |
1576 unsigned int last_segment; |
1521 unsigned int last_segment; |
1577 |
1522 |
1578 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1523 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1579 // FIXME: request again? |
1524 return; // FIXME: request again? |
1580 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1581 return; |
|
1582 } |
|
1583 |
1525 |
1584 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1526 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1585 fsm->state = ec_fsm_coe_error; |
1527 fsm->state = ec_fsm_coe_error; |
1586 EC_ERR("Failed to receive CoE upload segment response datagram for" |
1528 EC_ERR("Failed to receive CoE upload segment response datagram for" |
1587 " slave %i.\n", slave->ring_position); |
1529 " slave %i (datagram state %i).\n", |
|
1530 slave->ring_position, datagram->state); |
1588 return; |
1531 return; |
1589 } |
1532 } |
1590 |
1533 |
1591 if (datagram->working_counter != 1) { |
1534 if (datagram->working_counter != 1) { |
1592 fsm->state = ec_fsm_coe_error; |
1535 fsm->state = ec_fsm_coe_error; |
1666 if (master->debug_level) { |
1609 if (master->debug_level) { |
1667 EC_DBG("Upload segment request:\n"); |
1610 EC_DBG("Upload segment request:\n"); |
1668 ec_print_data(data, 3); |
1611 ec_print_data(data, 3); |
1669 } |
1612 } |
1670 |
1613 |
1671 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1672 fsm->retries = EC_FSM_RETRIES; |
1614 fsm->retries = EC_FSM_RETRIES; |
1673 fsm->state = ec_fsm_coe_up_seg_request; |
1615 fsm->state = ec_fsm_coe_up_seg_request; |
1674 return; |
1616 return; |
1675 } |
1617 } |
1676 |
1618 |