284 CoE state: DICT START. |
284 CoE state: DICT START. |
285 */ |
285 */ |
286 |
286 |
287 void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */) |
287 void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */) |
288 { |
288 { |
289 ec_datagram_t *datagram = fsm->datagram; |
289 ec_mailbox_t *mbox = fsm->mbox; |
290 ec_slave_t *slave = fsm->slave; |
290 ec_slave_t *slave = fsm->slave; |
291 uint8_t *data; |
291 uint8_t *data; |
292 |
292 |
293 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
293 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
294 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
294 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
326 \todo Timeout behavior |
326 \todo Timeout behavior |
327 */ |
327 */ |
328 |
328 |
329 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
329 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
330 { |
330 { |
331 ec_datagram_t *datagram = fsm->datagram; |
331 ec_mailbox_t *mbox = fsm->mbox; |
|
332 ec_datagram_t *datagram = mbox->datagram; |
332 ec_slave_t *slave = fsm->slave; |
333 ec_slave_t *slave = fsm->slave; |
333 |
334 |
334 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
335 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
335 return; // FIXME: request again? |
336 return; // FIXME: request again? |
336 |
337 |
337 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
338 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
338 fsm->state = ec_fsm_coe_error; |
339 fsm->state = ec_fsm_coe_error; |
339 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
340 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
340 " request datagram: "); |
341 " request datagram: "); |
341 ec_datagram_print_state(datagram); |
342 ec_datagram_print_state(datagram); |
342 return; |
343 return; |
343 } |
344 } |
344 |
345 |
345 if (datagram->working_counter != 1) { |
346 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
346 fsm->state = ec_fsm_coe_error; |
347 fsm->state = ec_fsm_coe_error; |
347 EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: "); |
348 EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: "); |
348 ec_datagram_print_wc_error(datagram); |
349 ec_datagram_print_wc_error(datagram); |
349 return; |
350 return; |
350 } |
351 } |
351 |
352 |
352 fsm->jiffies_start = datagram->jiffies_sent; |
353 fsm->jiffies_start = datagram->jiffies_sent; |
353 |
354 |
354 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
355 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
355 fsm->retries = EC_FSM_RETRIES; |
356 fsm->retries = EC_FSM_RETRIES; |
356 fsm->state = ec_fsm_coe_dict_check; |
357 fsm->state = ec_fsm_coe_dict_check; |
357 } |
358 } |
358 |
359 |
359 /*****************************************************************************/ |
360 /*****************************************************************************/ |
362 CoE state: DICT CHECK. |
363 CoE state: DICT CHECK. |
363 */ |
364 */ |
364 |
365 |
365 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
366 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
366 { |
367 { |
367 ec_datagram_t *datagram = fsm->datagram; |
368 ec_mailbox_t *mbox = fsm->mbox; |
|
369 ec_datagram_t *datagram = mbox->datagram; |
368 ec_slave_t *slave = fsm->slave; |
370 ec_slave_t *slave = fsm->slave; |
369 |
371 |
370 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
372 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
371 return; |
373 return; |
372 |
374 |
373 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
375 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
374 fsm->state = ec_fsm_coe_error; |
376 fsm->state = ec_fsm_coe_error; |
375 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
377 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
376 ec_datagram_print_state(datagram); |
378 ec_datagram_print_state(datagram); |
377 return; |
379 return; |
378 } |
380 } |
379 |
381 |
380 if (datagram->working_counter != 1) { |
382 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
381 fsm->state = ec_fsm_coe_error; |
383 fsm->state = ec_fsm_coe_error; |
382 EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" |
384 EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" |
383 " datagram failed: "); |
385 " datagram failed: "); |
384 ec_datagram_print_wc_error(datagram); |
386 ec_datagram_print_wc_error(datagram); |
385 return; |
387 return; |
386 } |
388 } |
387 |
389 |
388 if (!ec_slave_mbox_check(datagram)) { |
390 if (!ec_slave_mbox_check(mbox)) { |
389 unsigned long diff_ms = |
391 unsigned long diff_ms = |
390 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
392 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
391 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
393 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
392 fsm->state = ec_fsm_coe_error; |
394 fsm->state = ec_fsm_coe_error; |
393 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
395 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
394 " SDO dictionary list response.\n"); |
396 " SDO dictionary list response.\n"); |
395 return; |
397 return; |
396 } |
398 } |
397 |
399 |
398 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
400 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
399 fsm->retries = EC_FSM_RETRIES; |
401 fsm->retries = EC_FSM_RETRIES; |
400 return; |
402 return; |
401 } |
403 } |
402 |
404 |
403 // Fetch response |
405 // Fetch response |
404 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
406 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
405 fsm->retries = EC_FSM_RETRIES; |
407 fsm->retries = EC_FSM_RETRIES; |
406 fsm->state = ec_fsm_coe_dict_response; |
408 fsm->state = ec_fsm_coe_dict_response; |
407 } |
409 } |
408 |
410 |
409 /*****************************************************************************/ |
411 /*****************************************************************************/ |
413 \todo Timeout behavior |
415 \todo Timeout behavior |
414 */ |
416 */ |
415 |
417 |
416 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
418 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
417 { |
419 { |
418 ec_datagram_t *datagram = fsm->datagram; |
420 ec_mailbox_t *mbox = fsm->mbox; |
|
421 ec_datagram_t *datagram = mbox->datagram; |
419 ec_slave_t *slave = fsm->slave; |
422 ec_slave_t *slave = fsm->slave; |
420 uint8_t *data, mbox_prot; |
423 uint8_t *data, mbox_prot; |
421 size_t rec_size; |
424 size_t rec_size; |
422 unsigned int sdo_count, i; |
425 unsigned int sdo_count, i; |
423 uint16_t sdo_index, fragments_left; |
426 uint16_t sdo_index, fragments_left; |
424 ec_sdo_t *sdo; |
427 ec_sdo_t *sdo; |
425 |
428 bool first_segment; |
426 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
429 size_t index_list_offset; |
|
430 |
|
431 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
427 return; // FIXME: request again? |
432 return; // FIXME: request again? |
428 |
433 |
429 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
434 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
430 fsm->state = ec_fsm_coe_error; |
435 fsm->state = ec_fsm_coe_error; |
431 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
436 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
432 " response datagram: "); |
437 " response datagram: "); |
433 ec_datagram_print_state(datagram); |
438 ec_datagram_print_state(datagram); |
434 return; |
439 return; |
435 } |
440 } |
436 |
441 |
437 if (datagram->working_counter != 1) { |
442 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
438 fsm->state = ec_fsm_coe_error; |
443 fsm->state = ec_fsm_coe_error; |
439 EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); |
444 EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); |
440 ec_datagram_print_wc_error(datagram); |
445 ec_datagram_print_wc_error(datagram); |
441 return; |
446 return; |
442 } |
447 } |
443 |
448 |
444 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
449 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
445 if (IS_ERR(data)) { |
450 if (IS_ERR(data)) { |
446 fsm->state = ec_fsm_coe_error; |
451 fsm->state = ec_fsm_coe_error; |
447 return; |
452 return; |
448 } |
453 } |
449 |
454 |
488 if (fsm->slave->master->debug_level) { |
493 if (fsm->slave->master->debug_level) { |
489 EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!" |
494 EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!" |
490 " Retrying...\n"); |
495 " Retrying...\n"); |
491 ec_print_data(data, rec_size); |
496 ec_print_data(data, rec_size); |
492 } |
497 } |
493 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
498 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
494 fsm->retries = EC_FSM_RETRIES; |
499 fsm->retries = EC_FSM_RETRIES; |
495 fsm->state = ec_fsm_coe_dict_check; |
500 fsm->state = ec_fsm_coe_dict_check; |
496 return; |
501 return; |
497 } |
502 } |
498 |
503 |
501 ec_print_data(data, rec_size); |
506 ec_print_data(data, rec_size); |
502 fsm->state = ec_fsm_coe_error; |
507 fsm->state = ec_fsm_coe_error; |
503 return; |
508 return; |
504 } |
509 } |
505 |
510 |
506 sdo_count = (rec_size - 8) / 2; |
511 first_segment = list_empty(&slave->sdo_dictionary) ? true : false; |
|
512 index_list_offset = first_segment ? 8 : 6; |
|
513 |
|
514 sdo_count = (rec_size - index_list_offset) / 2; |
507 |
515 |
508 for (i = 0; i < sdo_count; i++) { |
516 for (i = 0; i < sdo_count; i++) { |
509 sdo_index = EC_READ_U16(data + 8 + i * 2); |
517 sdo_index = EC_READ_U16(data + index_list_offset + i * 2); |
510 if (!sdo_index) { |
518 if (!sdo_index) { |
511 EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n"); |
519 EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n"); |
512 continue; |
520 continue; |
513 } |
521 } |
514 |
522 |
569 \todo Timeout behavior |
577 \todo Timeout behavior |
570 */ |
578 */ |
571 |
579 |
572 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
580 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
573 { |
581 { |
574 ec_datagram_t *datagram = fsm->datagram; |
582 ec_mailbox_t *mbox = fsm->mbox; |
|
583 ec_datagram_t *datagram = mbox->datagram; |
575 ec_slave_t *slave = fsm->slave; |
584 ec_slave_t *slave = fsm->slave; |
576 |
585 |
577 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
586 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
578 return; // FIXME: check for response first? |
587 return; // FIXME: check for response first? |
579 |
588 |
580 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
589 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
581 fsm->state = ec_fsm_coe_error; |
590 fsm->state = ec_fsm_coe_error; |
582 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
591 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
583 " description request datagram: "); |
592 " description request datagram: "); |
584 ec_datagram_print_state(datagram); |
593 ec_datagram_print_state(datagram); |
585 return; |
594 return; |
586 } |
595 } |
587 |
596 |
588 if (datagram->working_counter != 1) { |
597 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
589 fsm->state = ec_fsm_coe_error; |
598 fsm->state = ec_fsm_coe_error; |
590 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
599 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
591 " request failed: "); |
600 " request failed: "); |
592 ec_datagram_print_wc_error(datagram); |
601 ec_datagram_print_wc_error(datagram); |
593 return; |
602 return; |
594 } |
603 } |
595 |
604 |
596 fsm->jiffies_start = datagram->jiffies_sent; |
605 fsm->jiffies_start = datagram->jiffies_sent; |
597 |
606 |
598 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
607 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
599 fsm->retries = EC_FSM_RETRIES; |
608 fsm->retries = EC_FSM_RETRIES; |
600 fsm->state = ec_fsm_coe_dict_desc_check; |
609 fsm->state = ec_fsm_coe_dict_desc_check; |
601 } |
610 } |
602 |
611 |
603 /*****************************************************************************/ |
612 /*****************************************************************************/ |
606 CoE state: DICT DESC CHECK. |
615 CoE state: DICT DESC CHECK. |
607 */ |
616 */ |
608 |
617 |
609 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
618 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
610 { |
619 { |
611 ec_datagram_t *datagram = fsm->datagram; |
620 ec_mailbox_t *mbox = fsm->mbox; |
|
621 ec_datagram_t *datagram = mbox->datagram; |
612 ec_slave_t *slave = fsm->slave; |
622 ec_slave_t *slave = fsm->slave; |
613 |
623 |
614 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
624 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
615 return; |
625 return; |
616 |
626 |
617 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
627 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
618 fsm->state = ec_fsm_coe_error; |
628 fsm->state = ec_fsm_coe_error; |
619 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
629 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
620 ec_datagram_print_state(datagram); |
630 ec_datagram_print_state(datagram); |
621 return; |
631 return; |
622 } |
632 } |
623 |
633 |
624 if (datagram->working_counter != 1) { |
634 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
625 fsm->state = ec_fsm_coe_error; |
635 fsm->state = ec_fsm_coe_error; |
626 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
636 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
627 " datagram failed: "); |
637 " datagram failed: "); |
628 ec_datagram_print_wc_error(datagram); |
638 ec_datagram_print_wc_error(datagram); |
629 return; |
639 return; |
630 } |
640 } |
631 |
641 |
632 if (!ec_slave_mbox_check(datagram)) { |
642 if (!ec_slave_mbox_check(mbox)) { |
633 unsigned long diff_ms = |
643 unsigned long diff_ms = |
634 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
644 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
635 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
645 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
636 fsm->state = ec_fsm_coe_error; |
646 fsm->state = ec_fsm_coe_error; |
637 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
647 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
638 " SDO 0x%04x object description response.\n", |
648 " SDO 0x%04x object description response.\n", |
639 fsm->sdo->index); |
649 fsm->sdo->index); |
640 return; |
650 return; |
641 } |
651 } |
642 |
652 |
643 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
653 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
644 fsm->retries = EC_FSM_RETRIES; |
654 fsm->retries = EC_FSM_RETRIES; |
645 return; |
655 return; |
646 } |
656 } |
647 |
657 |
648 // Fetch response |
658 // Fetch response |
649 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
659 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
650 fsm->retries = EC_FSM_RETRIES; |
660 fsm->retries = EC_FSM_RETRIES; |
651 fsm->state = ec_fsm_coe_dict_desc_response; |
661 fsm->state = ec_fsm_coe_dict_desc_response; |
652 } |
662 } |
653 |
663 |
654 /*****************************************************************************/ |
664 /*****************************************************************************/ |
659 */ |
669 */ |
660 |
670 |
661 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm |
671 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm |
662 /**< finite state machine */) |
672 /**< finite state machine */) |
663 { |
673 { |
664 ec_datagram_t *datagram = fsm->datagram; |
674 ec_mailbox_t *mbox = fsm->mbox; |
|
675 ec_datagram_t *datagram = mbox->datagram; |
665 ec_slave_t *slave = fsm->slave; |
676 ec_slave_t *slave = fsm->slave; |
666 ec_sdo_t *sdo = fsm->sdo; |
677 ec_sdo_t *sdo = fsm->sdo; |
667 uint8_t *data, mbox_prot; |
678 uint8_t *data, mbox_prot; |
668 size_t rec_size, name_size; |
679 size_t rec_size, name_size; |
669 |
680 |
670 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
681 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
671 return; // FIXME: request again? |
682 return; // FIXME: request again? |
672 |
683 |
673 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
684 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
674 fsm->state = ec_fsm_coe_error; |
685 fsm->state = ec_fsm_coe_error; |
675 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" |
686 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" |
676 " response datagram: "); |
687 " response datagram: "); |
677 ec_datagram_print_state(datagram); |
688 ec_datagram_print_state(datagram); |
678 return; |
689 return; |
679 } |
690 } |
680 |
691 |
681 if (datagram->working_counter != 1) { |
692 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
682 fsm->state = ec_fsm_coe_error; |
693 fsm->state = ec_fsm_coe_error; |
683 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
694 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
684 " response failed: "); |
695 " response failed: "); |
685 ec_datagram_print_wc_error(datagram); |
696 ec_datagram_print_wc_error(datagram); |
686 return; |
697 return; |
687 } |
698 } |
688 |
699 |
689 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
700 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
690 if (IS_ERR(data)) { |
701 if (IS_ERR(data)) { |
691 fsm->state = ec_fsm_coe_error; |
702 fsm->state = ec_fsm_coe_error; |
692 return; |
703 return; |
693 } |
704 } |
694 |
705 |
737 EC_SLAVE_DBG(slave, 1, "Invalid object description response while" |
748 EC_SLAVE_DBG(slave, 1, "Invalid object description response while" |
738 " fetching SDO 0x%04X!\n", sdo->index); |
749 " fetching SDO 0x%04X!\n", sdo->index); |
739 ec_print_data(data, rec_size); |
750 ec_print_data(data, rec_size); |
740 } |
751 } |
741 // check for CoE response again |
752 // check for CoE response again |
742 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
753 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
743 fsm->retries = EC_FSM_RETRIES; |
754 fsm->retries = EC_FSM_RETRIES; |
744 fsm->state = ec_fsm_coe_dict_desc_check; |
755 fsm->state = ec_fsm_coe_dict_desc_check; |
745 return; |
756 return; |
746 } |
757 } |
747 |
758 |
803 */ |
814 */ |
804 |
815 |
805 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm |
816 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm |
806 /**< finite state machine */) |
817 /**< finite state machine */) |
807 { |
818 { |
808 ec_datagram_t *datagram = fsm->datagram; |
819 ec_mailbox_t *mbox = fsm->mbox; |
|
820 ec_datagram_t *datagram = mbox->datagram; |
809 ec_slave_t *slave = fsm->slave; |
821 ec_slave_t *slave = fsm->slave; |
810 |
822 |
811 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
823 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
812 return; // FIXME: check for response first? |
824 return; // FIXME: check for response first? |
813 |
825 |
814 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
826 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
815 fsm->state = ec_fsm_coe_error; |
827 fsm->state = ec_fsm_coe_error; |
816 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry" |
828 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry" |
817 " request datagram: "); |
829 " request datagram: "); |
818 ec_datagram_print_state(datagram); |
830 ec_datagram_print_state(datagram); |
819 return; |
831 return; |
820 } |
832 } |
821 |
833 |
822 if (datagram->working_counter != 1) { |
834 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
823 fsm->state = ec_fsm_coe_error; |
835 fsm->state = ec_fsm_coe_error; |
824 EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: "); |
836 EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: "); |
825 ec_datagram_print_wc_error(datagram); |
837 ec_datagram_print_wc_error(datagram); |
826 return; |
838 return; |
827 } |
839 } |
828 |
840 |
829 fsm->jiffies_start = datagram->jiffies_sent; |
841 fsm->jiffies_start = datagram->jiffies_sent; |
830 |
842 |
831 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
843 ec_slave_mbox_prepare_check(slave, mbox); // can not fail |
832 fsm->retries = EC_FSM_RETRIES; |
844 fsm->retries = EC_FSM_RETRIES; |
833 fsm->state = ec_fsm_coe_dict_entry_check; |
845 fsm->state = ec_fsm_coe_dict_entry_check; |
834 } |
846 } |
835 |
847 |
836 /*****************************************************************************/ |
848 /*****************************************************************************/ |
840 */ |
852 */ |
841 |
853 |
842 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm |
854 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm |
843 /**< finite state machine */) |
855 /**< finite state machine */) |
844 { |
856 { |
845 ec_datagram_t *datagram = fsm->datagram; |
857 ec_mailbox_t *mbox = fsm->mbox; |
|
858 ec_datagram_t *datagram = mbox->datagram; |
846 ec_slave_t *slave = fsm->slave; |
859 ec_slave_t *slave = fsm->slave; |
847 |
860 |
848 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
861 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
849 return; |
862 return; |
850 |
863 |
851 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
864 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
852 fsm->state = ec_fsm_coe_error; |
865 fsm->state = ec_fsm_coe_error; |
853 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
866 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
854 ec_datagram_print_state(datagram); |
867 ec_datagram_print_state(datagram); |
855 return; |
868 return; |
856 } |
869 } |
857 |
870 |
858 if (datagram->working_counter != 1) { |
871 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
859 fsm->state = ec_fsm_coe_error; |
872 fsm->state = ec_fsm_coe_error; |
860 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
873 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
861 " datagram failed: "); |
874 " datagram failed: "); |
862 ec_datagram_print_wc_error(datagram); |
875 ec_datagram_print_wc_error(datagram); |
863 return; |
876 return; |
864 } |
877 } |
865 |
878 |
866 if (!ec_slave_mbox_check(datagram)) { |
879 if (!ec_slave_mbox_check(mbox)) { |
867 unsigned long diff_ms = |
880 unsigned long diff_ms = |
868 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
881 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
869 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
882 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
870 fsm->state = ec_fsm_coe_error; |
883 fsm->state = ec_fsm_coe_error; |
871 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
884 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
872 " SDO entry 0x%04x:%x description response.\n", |
885 " SDO entry 0x%04x:%x description response.\n", |
873 fsm->sdo->index, fsm->subindex); |
886 fsm->sdo->index, fsm->subindex); |
874 return; |
887 return; |
875 } |
888 } |
876 |
889 |
877 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
890 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
878 fsm->retries = EC_FSM_RETRIES; |
891 fsm->retries = EC_FSM_RETRIES; |
879 return; |
892 return; |
880 } |
893 } |
881 |
894 |
882 // Fetch response |
895 // Fetch response |
883 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
896 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
884 fsm->retries = EC_FSM_RETRIES; |
897 fsm->retries = EC_FSM_RETRIES; |
885 fsm->state = ec_fsm_coe_dict_entry_response; |
898 fsm->state = ec_fsm_coe_dict_entry_response; |
886 } |
899 } |
887 |
900 |
888 /*****************************************************************************/ |
901 /*****************************************************************************/ |
893 */ |
906 */ |
894 |
907 |
895 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm |
908 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm |
896 /**< finite state machine */) |
909 /**< finite state machine */) |
897 { |
910 { |
898 ec_datagram_t *datagram = fsm->datagram; |
911 ec_mailbox_t *mbox = fsm->mbox; |
|
912 ec_datagram_t *datagram = mbox->datagram; |
899 ec_slave_t *slave = fsm->slave; |
913 ec_slave_t *slave = fsm->slave; |
900 ec_sdo_t *sdo = fsm->sdo; |
914 ec_sdo_t *sdo = fsm->sdo; |
901 uint8_t *data, mbox_prot; |
915 uint8_t *data, mbox_prot; |
902 size_t rec_size, data_size; |
916 size_t rec_size, data_size; |
903 ec_sdo_entry_t *entry; |
917 ec_sdo_entry_t *entry; |
904 u16 word; |
918 u16 word; |
905 |
919 |
906 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
920 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
907 return; // FIXME: request again? |
921 return; // FIXME: request again? |
908 |
922 |
909 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
923 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
910 fsm->state = ec_fsm_coe_error; |
924 fsm->state = ec_fsm_coe_error; |
911 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
925 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
912 " description response datagram: "); |
926 " description response datagram: "); |
913 ec_datagram_print_state(datagram); |
927 ec_datagram_print_state(datagram); |
914 return; |
928 return; |
915 } |
929 } |
916 |
930 |
917 if (datagram->working_counter != 1) { |
931 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
918 fsm->state = ec_fsm_coe_error; |
932 fsm->state = ec_fsm_coe_error; |
919 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
933 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
920 " response failed: "); |
934 " response failed: "); |
921 ec_datagram_print_wc_error(datagram); |
935 ec_datagram_print_wc_error(datagram); |
922 return; |
936 return; |
923 } |
937 } |
924 |
938 |
925 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
939 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
926 if (IS_ERR(data)) { |
940 if (IS_ERR(data)) { |
927 fsm->state = ec_fsm_coe_error; |
941 fsm->state = ec_fsm_coe_error; |
928 return; |
942 return; |
929 } |
943 } |
930 |
944 |
976 " fetching SDO entry 0x%04X:%02X!\n", |
990 " fetching SDO entry 0x%04X:%02X!\n", |
977 sdo->index, fsm->subindex); |
991 sdo->index, fsm->subindex); |
978 ec_print_data(data, rec_size); |
992 ec_print_data(data, rec_size); |
979 } |
993 } |
980 // check for CoE response again |
994 // check for CoE response again |
981 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
995 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
982 fsm->retries = EC_FSM_RETRIES; |
996 fsm->retries = EC_FSM_RETRIES; |
983 fsm->state = ec_fsm_coe_dict_entry_check; |
997 fsm->state = ec_fsm_coe_dict_entry_check; |
984 return; |
998 return; |
985 } |
999 } |
986 |
1000 |
1051 |
1065 |
1052 // another SDO description to fetch? |
1066 // another SDO description to fetch? |
1053 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
1067 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
1054 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
1068 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
1055 |
1069 |
1056 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); |
1070 data = ec_slave_mbox_prepare_send(slave, mbox, 0x03, 8); |
1057 if (IS_ERR(data)) { |
1071 if (IS_ERR(data)) { |
1058 fsm->state = ec_fsm_coe_error; |
1072 fsm->state = ec_fsm_coe_error; |
1059 return; |
1073 return; |
1060 } |
1074 } |
1061 |
1075 |
1115 fsm->state = ec_fsm_coe_error; |
1129 fsm->state = ec_fsm_coe_error; |
1116 return; |
1130 return; |
1117 } |
1131 } |
1118 |
1132 |
1119 if (request->data_size <= 4) { // use expedited transfer type |
1133 if (request->data_size <= 4) { // use expedited transfer type |
1120 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1134 data = ec_slave_mbox_prepare_send(slave, mbox, 0x03, |
1121 EC_COE_DOWN_REQ_HEADER_SIZE); |
1135 EC_COE_DOWN_REQ_HEADER_SIZE); |
1122 if (IS_ERR(data)) { |
1136 if (IS_ERR(data)) { |
1123 request->errno = PTR_ERR(data); |
1137 request->errno = PTR_ERR(data); |
1124 fsm->state = ec_fsm_coe_error; |
1138 fsm->state = ec_fsm_coe_error; |
1125 return; |
1139 return; |
1206 \todo Timeout behavior |
1220 \todo Timeout behavior |
1207 */ |
1221 */ |
1208 |
1222 |
1209 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1223 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1210 { |
1224 { |
1211 ec_datagram_t *datagram = fsm->datagram; |
1225 ec_mailbox_t *mbox = fsm->mbox; |
|
1226 ec_datagram_t *datagram = mbox->datagram; |
1212 ec_slave_t *slave = fsm->slave; |
1227 ec_slave_t *slave = fsm->slave; |
1213 unsigned long diff_ms; |
1228 unsigned long diff_ms; |
1214 |
1229 |
1215 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1230 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1216 return; // FIXME: check for response first? |
1231 return; // FIXME: check for response first? |
1217 |
1232 |
1218 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1233 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1219 fsm->request->errno = EIO; |
1234 fsm->request->errno = EIO; |
1220 fsm->state = ec_fsm_coe_error; |
1235 fsm->state = ec_fsm_coe_error; |
1221 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1236 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1222 " request datagram: "); |
1237 " request datagram: "); |
1223 ec_datagram_print_state(datagram); |
1238 ec_datagram_print_state(datagram); |
1224 return; |
1239 return; |
1225 } |
1240 } |
1226 |
1241 |
1227 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1242 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1228 |
1243 |
1229 if (datagram->working_counter != 1) { |
1244 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1230 if (!datagram->working_counter) { |
1245 if (ec_mbox_is_datagram_wc(mbox,0)) { |
1231 if (diff_ms < fsm->request->response_timeout) { |
1246 if (diff_ms < fsm->request->response_timeout) { |
1232 #if DEBUG_RETRIES |
1247 #if DEBUG_RETRIES |
1233 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO" |
1248 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO" |
1234 " download request. Retrying after %lu ms...\n", |
1249 " download request. Retrying after %lu ms...\n", |
1235 diff_ms); |
1250 diff_ms); |
1254 } |
1269 } |
1255 #endif |
1270 #endif |
1256 |
1271 |
1257 fsm->jiffies_start = datagram->jiffies_sent; |
1272 fsm->jiffies_start = datagram->jiffies_sent; |
1258 |
1273 |
1259 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1274 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1260 fsm->retries = EC_FSM_RETRIES; |
1275 fsm->retries = EC_FSM_RETRIES; |
1261 fsm->state = ec_fsm_coe_down_check; |
1276 fsm->state = ec_fsm_coe_down_check; |
1262 } |
1277 } |
1263 |
1278 |
1264 /*****************************************************************************/ |
1279 /*****************************************************************************/ |
1265 |
1280 |
1266 /** CoE state: DOWN CHECK. |
1281 /** CoE state: DOWN CHECK. |
1267 */ |
1282 */ |
1268 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1283 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1269 { |
1284 { |
1270 ec_datagram_t *datagram = fsm->datagram; |
1285 ec_mailbox_t *mbox = fsm->mbox; |
|
1286 ec_datagram_t *datagram = mbox->datagram; |
1271 ec_slave_t *slave = fsm->slave; |
1287 ec_slave_t *slave = fsm->slave; |
1272 |
1288 |
1273 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1289 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1274 return; |
1290 return; |
1275 |
1291 |
1276 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1292 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1277 fsm->request->errno = EIO; |
1293 fsm->request->errno = EIO; |
1278 fsm->state = ec_fsm_coe_error; |
1294 fsm->state = ec_fsm_coe_error; |
1279 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
1295 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
1280 " datagram: "); |
1296 " datagram: "); |
1281 ec_datagram_print_state(datagram); |
1297 ec_datagram_print_state(datagram); |
1282 return; |
1298 return; |
1283 } |
1299 } |
1284 |
1300 |
1285 if (datagram->working_counter != 1) { |
1301 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1286 fsm->request->errno = EIO; |
1302 fsm->request->errno = EIO; |
1287 fsm->state = ec_fsm_coe_error; |
1303 fsm->state = ec_fsm_coe_error; |
1288 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1304 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1289 " datagram failed: "); |
1305 " datagram failed: "); |
1290 ec_datagram_print_wc_error(datagram); |
1306 ec_datagram_print_wc_error(datagram); |
1291 return; |
1307 return; |
1292 } |
1308 } |
1293 |
1309 |
1294 if (!ec_slave_mbox_check(datagram)) { |
1310 if (!ec_slave_mbox_check(mbox)) { |
1295 unsigned long diff_ms = |
1311 unsigned long diff_ms = |
1296 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1312 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1297 if (diff_ms >= fsm->request->response_timeout) { |
1313 if (diff_ms >= fsm->request->response_timeout) { |
1298 fsm->request->errno = EIO; |
1314 fsm->request->errno = EIO; |
1299 fsm->state = ec_fsm_coe_error; |
1315 fsm->state = ec_fsm_coe_error; |
1301 " for SDO 0x%04x:%x download response.\n", diff_ms, |
1317 " for SDO 0x%04x:%x download response.\n", diff_ms, |
1302 fsm->request->index, fsm->request->subindex); |
1318 fsm->request->index, fsm->request->subindex); |
1303 return; |
1319 return; |
1304 } |
1320 } |
1305 |
1321 |
1306 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1322 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1307 fsm->retries = EC_FSM_RETRIES; |
1323 fsm->retries = EC_FSM_RETRIES; |
1308 return; |
1324 return; |
1309 } |
1325 } |
1310 |
1326 |
1311 // Fetch response |
1327 // Fetch response |
1312 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1328 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
1313 fsm->retries = EC_FSM_RETRIES; |
1329 fsm->retries = EC_FSM_RETRIES; |
1314 fsm->state = ec_fsm_coe_down_response; |
1330 fsm->state = ec_fsm_coe_down_response; |
1315 } |
1331 } |
1316 |
1332 |
1317 /*****************************************************************************/ |
1333 /*****************************************************************************/ |
1320 */ |
1336 */ |
1321 void ec_fsm_coe_down_prepare_segment_request( |
1337 void ec_fsm_coe_down_prepare_segment_request( |
1322 ec_fsm_coe_t *fsm /**< finite state machine */ |
1338 ec_fsm_coe_t *fsm /**< finite state machine */ |
1323 ) |
1339 ) |
1324 { |
1340 { |
1325 ec_datagram_t *datagram = fsm->datagram; |
1341 ec_mailbox_t *mbox = fsm->mbox; |
1326 ec_slave_t *slave = fsm->slave; |
1342 ec_slave_t *slave = fsm->slave; |
1327 ec_sdo_request_t *request = fsm->request; |
1343 ec_sdo_request_t *request = fsm->request; |
1328 size_t max_segment_size = |
1344 size_t max_segment_size = |
1329 slave->configured_rx_mailbox_size |
1345 slave->configured_rx_mailbox_size |
1330 - EC_MBOX_HEADER_SIZE |
1346 - EC_MBOX_HEADER_SIZE |
1347 seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size; |
1363 seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size; |
1348 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE |
1364 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE |
1349 + EC_COE_DOWN_SEG_MIN_DATA_SIZE; |
1365 + EC_COE_DOWN_SEG_MIN_DATA_SIZE; |
1350 } |
1366 } |
1351 |
1367 |
1352 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1368 data = ec_slave_mbox_prepare_send(slave, mbox, 0x03, |
1353 data_size); |
1369 data_size); |
1354 if (IS_ERR(data)) { |
1370 if (IS_ERR(data)) { |
1355 request->errno = PTR_ERR(data); |
1371 request->errno = PTR_ERR(data); |
1356 fsm->state = ec_fsm_coe_error; |
1372 fsm->state = ec_fsm_coe_error; |
1357 return; |
1373 return; |
1387 \todo Timeout behavior |
1403 \todo Timeout behavior |
1388 */ |
1404 */ |
1389 |
1405 |
1390 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1406 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1391 { |
1407 { |
1392 ec_datagram_t *datagram = fsm->datagram; |
1408 ec_mailbox_t *mbox = fsm->mbox; |
|
1409 ec_datagram_t *datagram = fsm->mbox->datagram; |
1393 ec_slave_t *slave = fsm->slave; |
1410 ec_slave_t *slave = fsm->slave; |
1394 uint8_t *data, mbox_prot; |
1411 uint8_t *data, mbox_prot; |
1395 size_t rec_size; |
1412 size_t rec_size; |
1396 ec_sdo_request_t *request = fsm->request; |
1413 ec_sdo_request_t *request = fsm->request; |
1397 |
1414 |
1398 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1415 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1399 return; // FIXME: request again? |
1416 return; // FIXME: request again? |
1400 |
1417 |
1401 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1418 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1402 request->errno = EIO; |
1419 request->errno = EIO; |
1403 fsm->state = ec_fsm_coe_error; |
1420 fsm->state = ec_fsm_coe_error; |
1404 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1421 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1405 " response datagram: "); |
1422 " response datagram: "); |
1406 ec_datagram_print_state(datagram); |
1423 ec_datagram_print_state(datagram); |
1407 return; |
1424 return; |
1408 } |
1425 } |
1409 |
1426 |
1410 if (datagram->working_counter != 1) { |
1427 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1411 request->errno = EIO; |
1428 request->errno = EIO; |
1412 fsm->state = ec_fsm_coe_error; |
1429 fsm->state = ec_fsm_coe_error; |
1413 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1430 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1414 ec_datagram_print_wc_error(datagram); |
1431 ec_datagram_print_wc_error(datagram); |
1415 return; |
1432 return; |
1416 } |
1433 } |
1417 |
1434 |
1418 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1435 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
1419 if (IS_ERR(data)) { |
1436 if (IS_ERR(data)) { |
1420 request->errno = PTR_ERR(data); |
1437 request->errno = PTR_ERR(data); |
1421 fsm->state = ec_fsm_coe_error; |
1438 fsm->state = ec_fsm_coe_error; |
1422 return; |
1439 return; |
1423 } |
1440 } |
1482 EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" |
1499 EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" |
1483 " Retrying...\n"); |
1500 " Retrying...\n"); |
1484 ec_print_data(data, rec_size); |
1501 ec_print_data(data, rec_size); |
1485 } |
1502 } |
1486 // check for CoE response again |
1503 // check for CoE response again |
1487 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1504 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1488 fsm->retries = EC_FSM_RETRIES; |
1505 fsm->retries = EC_FSM_RETRIES; |
1489 fsm->state = ec_fsm_coe_down_check; |
1506 fsm->state = ec_fsm_coe_down_check; |
1490 return; |
1507 return; |
1491 } |
1508 } |
1492 |
1509 |
1504 CoE state: DOWN SEG CHECK. |
1521 CoE state: DOWN SEG CHECK. |
1505 */ |
1522 */ |
1506 |
1523 |
1507 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1524 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1508 { |
1525 { |
1509 ec_datagram_t *datagram = fsm->datagram; |
1526 ec_mailbox_t *mbox = fsm->mbox; |
|
1527 ec_datagram_t *datagram = mbox->datagram; |
1510 ec_slave_t *slave = fsm->slave; |
1528 ec_slave_t *slave = fsm->slave; |
1511 |
1529 |
1512 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1530 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1513 return; |
1531 return; |
1514 |
1532 |
1515 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1533 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1516 fsm->request->errno = EIO; |
1534 fsm->request->errno = EIO; |
1517 fsm->state = ec_fsm_coe_error; |
1535 fsm->state = ec_fsm_coe_error; |
1518 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1536 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1519 ec_datagram_print_state(datagram); |
1537 ec_datagram_print_state(datagram); |
1520 return; |
1538 return; |
1521 } |
1539 } |
1522 |
1540 |
1523 if (datagram->working_counter != 1) { |
1541 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1524 fsm->request->errno = EIO; |
1542 fsm->request->errno = EIO; |
1525 fsm->state = ec_fsm_coe_error; |
1543 fsm->state = ec_fsm_coe_error; |
1526 EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" |
1544 EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" |
1527 " datagram failed: "); |
1545 " datagram failed: "); |
1528 ec_datagram_print_wc_error(datagram); |
1546 ec_datagram_print_wc_error(datagram); |
1529 return; |
1547 return; |
1530 } |
1548 } |
1531 |
1549 |
1532 if (!ec_slave_mbox_check(datagram)) { |
1550 if (!ec_slave_mbox_check(mbox)) { |
1533 unsigned long diff_ms = |
1551 unsigned long diff_ms = |
1534 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1552 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1535 if (diff_ms >= fsm->request->response_timeout) { |
1553 if (diff_ms >= fsm->request->response_timeout) { |
1536 fsm->request->errno = EIO; |
1554 fsm->request->errno = EIO; |
1537 fsm->state = ec_fsm_coe_error; |
1555 fsm->state = ec_fsm_coe_error; |
1538 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" |
1556 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" |
1539 " segment response.\n"); |
1557 " segment response.\n"); |
1540 return; |
1558 return; |
1541 } |
1559 } |
1542 |
1560 |
1543 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1561 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1544 fsm->retries = EC_FSM_RETRIES; |
1562 fsm->retries = EC_FSM_RETRIES; |
1545 return; |
1563 return; |
1546 } |
1564 } |
1547 |
1565 |
1548 // Fetch response |
1566 // Fetch response |
1549 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1567 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
1550 fsm->retries = EC_FSM_RETRIES; |
1568 fsm->retries = EC_FSM_RETRIES; |
1551 fsm->state = ec_fsm_coe_down_seg_response; |
1569 fsm->state = ec_fsm_coe_down_seg_response; |
1552 } |
1570 } |
1553 |
1571 |
1554 /*****************************************************************************/ |
1572 /*****************************************************************************/ |
1560 |
1578 |
1561 void ec_fsm_coe_down_seg_response( |
1579 void ec_fsm_coe_down_seg_response( |
1562 ec_fsm_coe_t *fsm /**< finite state machine */ |
1580 ec_fsm_coe_t *fsm /**< finite state machine */ |
1563 ) |
1581 ) |
1564 { |
1582 { |
1565 ec_datagram_t *datagram = fsm->datagram; |
1583 ec_mailbox_t *mbox = fsm->mbox; |
|
1584 ec_datagram_t *datagram = mbox->datagram; |
1566 ec_slave_t *slave = fsm->slave; |
1585 ec_slave_t *slave = fsm->slave; |
1567 uint8_t *data, mbox_prot; |
1586 uint8_t *data, mbox_prot; |
1568 size_t rec_size; |
1587 size_t rec_size; |
1569 ec_sdo_request_t *request = fsm->request; |
1588 ec_sdo_request_t *request = fsm->request; |
1570 |
1589 |
1571 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1590 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1572 return; // FIXME: request again? |
1591 return; // FIXME: request again? |
1573 |
1592 |
1574 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1593 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1575 request->errno = EIO; |
1594 request->errno = EIO; |
1576 fsm->state = ec_fsm_coe_error; |
1595 fsm->state = ec_fsm_coe_error; |
1577 EC_SLAVE_ERR(slave, "Failed to receive CoE download response" |
1596 EC_SLAVE_ERR(slave, "Failed to receive CoE download response" |
1578 " datagram: "); |
1597 " datagram: "); |
1579 ec_datagram_print_state(datagram); |
1598 ec_datagram_print_state(datagram); |
1580 return; |
1599 return; |
1581 } |
1600 } |
1582 |
1601 |
1583 if (datagram->working_counter != 1) { |
1602 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1584 request->errno = EIO; |
1603 request->errno = EIO; |
1585 fsm->state = ec_fsm_coe_error; |
1604 fsm->state = ec_fsm_coe_error; |
1586 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1605 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1587 ec_datagram_print_wc_error(datagram); |
1606 ec_datagram_print_wc_error(datagram); |
1588 return; |
1607 return; |
1589 } |
1608 } |
1590 |
1609 |
1591 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1610 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
1592 if (IS_ERR(data)) { |
1611 if (IS_ERR(data)) { |
1593 request->errno = PTR_ERR(data); |
1612 request->errno = PTR_ERR(data); |
1594 fsm->state = ec_fsm_coe_error; |
1613 fsm->state = ec_fsm_coe_error; |
1595 return; |
1614 return; |
1596 } |
1615 } |
1653 EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" |
1672 EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!" |
1654 " Retrying...\n"); |
1673 " Retrying...\n"); |
1655 ec_print_data(data, rec_size); |
1674 ec_print_data(data, rec_size); |
1656 } |
1675 } |
1657 // check for CoE response again |
1676 // check for CoE response again |
1658 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1677 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1659 fsm->retries = EC_FSM_RETRIES; |
1678 fsm->retries = EC_FSM_RETRIES; |
1660 fsm->state = ec_fsm_coe_down_seg_check; |
1679 fsm->state = ec_fsm_coe_down_seg_check; |
1661 return; |
1680 return; |
1662 } |
1681 } |
1663 |
1682 |
1732 \todo Timeout behavior |
1751 \todo Timeout behavior |
1733 */ |
1752 */ |
1734 |
1753 |
1735 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1754 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1736 { |
1755 { |
1737 ec_datagram_t *datagram = fsm->datagram; |
1756 ec_mailbox_t *mbox = fsm->mbox; |
|
1757 ec_datagram_t *datagram = mbox->datagram; |
1738 ec_slave_t *slave = fsm->slave; |
1758 ec_slave_t *slave = fsm->slave; |
1739 unsigned long diff_ms; |
1759 unsigned long diff_ms; |
1740 |
1760 |
1741 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1761 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1742 return; // FIXME: check for response first? |
1762 return; // FIXME: check for response first? |
1743 |
1763 |
1744 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1764 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1745 fsm->request->errno = EIO; |
1765 fsm->request->errno = EIO; |
1746 fsm->state = ec_fsm_coe_error; |
1766 fsm->state = ec_fsm_coe_error; |
1747 EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: "); |
1767 EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: "); |
1748 ec_datagram_print_state(datagram); |
1768 ec_datagram_print_state(datagram); |
1749 return; |
1769 return; |
1750 } |
1770 } |
1751 |
1771 |
1752 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1772 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1753 |
1773 |
1754 if (datagram->working_counter != 1) { |
1774 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1755 if (!datagram->working_counter) { |
1775 if (ec_mbox_is_datagram_wc(mbox,0)) { |
1756 if (diff_ms < fsm->request->response_timeout) { |
1776 if (diff_ms < fsm->request->response_timeout) { |
1757 #if DEBUG_RETRIES |
1777 #if DEBUG_RETRIES |
1758 EC_SLAVE_DBG(slave, 1, "Slave did not respond to" |
1778 EC_SLAVE_DBG(slave, 1, "Slave did not respond to" |
1759 " SDO upload request. Retrying after %lu ms...\n", |
1779 " SDO upload request. Retrying after %lu ms...\n", |
1760 diff_ms); |
1780 diff_ms); |
1792 CoE state: UP CHECK. |
1812 CoE state: UP CHECK. |
1793 */ |
1813 */ |
1794 |
1814 |
1795 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1815 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1796 { |
1816 { |
1797 ec_datagram_t *datagram = fsm->datagram; |
1817 ec_mailbox_t *mbox = fsm->mbox; |
|
1818 ec_datagram_t *datagram = mbox->datagram; |
1798 ec_slave_t *slave = fsm->slave; |
1819 ec_slave_t *slave = fsm->slave; |
1799 |
1820 |
1800 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1821 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1801 return; |
1822 return; |
1802 |
1823 |
1803 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1824 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1804 fsm->request->errno = EIO; |
1825 fsm->request->errno = EIO; |
1805 fsm->state = ec_fsm_coe_error; |
1826 fsm->state = ec_fsm_coe_error; |
1806 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1827 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1807 ec_datagram_print_state(datagram); |
1828 ec_datagram_print_state(datagram); |
1808 return; |
1829 return; |
1809 } |
1830 } |
1810 |
1831 |
1811 if (datagram->working_counter != 1) { |
1832 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1812 fsm->request->errno = EIO; |
1833 fsm->request->errno = EIO; |
1813 fsm->state = ec_fsm_coe_error; |
1834 fsm->state = ec_fsm_coe_error; |
1814 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1835 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1815 " datagram failed: "); |
1836 " datagram failed: "); |
1816 ec_datagram_print_wc_error(datagram); |
1837 ec_datagram_print_wc_error(datagram); |
1817 return; |
1838 return; |
1818 } |
1839 } |
1819 |
1840 |
1820 if (!ec_slave_mbox_check(datagram)) { |
1841 if (!ec_slave_mbox_check(mbox)) { |
1821 unsigned long diff_ms = |
1842 unsigned long diff_ms = |
1822 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1843 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1823 if (diff_ms >= fsm->request->response_timeout) { |
1844 if (diff_ms >= fsm->request->response_timeout) { |
1824 fsm->request->errno = EIO; |
1845 fsm->request->errno = EIO; |
1825 fsm->state = ec_fsm_coe_error; |
1846 fsm->state = ec_fsm_coe_error; |
1827 " SDO 0x%04x:%x upload response.\n", diff_ms, |
1848 " SDO 0x%04x:%x upload response.\n", diff_ms, |
1828 fsm->request->index, fsm->request->subindex); |
1849 fsm->request->index, fsm->request->subindex); |
1829 return; |
1850 return; |
1830 } |
1851 } |
1831 |
1852 |
1832 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1853 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1833 fsm->retries = EC_FSM_RETRIES; |
1854 fsm->retries = EC_FSM_RETRIES; |
1834 return; |
1855 return; |
1835 } |
1856 } |
1836 |
1857 |
1837 // Fetch response |
1858 // Fetch response |
1838 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1859 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
1839 fsm->retries = EC_FSM_RETRIES; |
1860 fsm->retries = EC_FSM_RETRIES; |
1840 fsm->state = ec_fsm_coe_up_response; |
1861 fsm->state = ec_fsm_coe_up_response; |
1841 } |
1862 } |
1842 |
1863 |
1843 /*****************************************************************************/ |
1864 /*****************************************************************************/ |
1874 \todo Timeout behavior |
1895 \todo Timeout behavior |
1875 */ |
1896 */ |
1876 |
1897 |
1877 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1898 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1878 { |
1899 { |
1879 ec_datagram_t *datagram = fsm->datagram; |
1900 ec_mailbox_t *mbox = fsm->mbox; |
|
1901 ec_datagram_t *datagram = mbox->datagram; |
1880 ec_slave_t *slave = fsm->slave; |
1902 ec_slave_t *slave = fsm->slave; |
1881 ec_master_t *master = slave->master; |
1903 ec_master_t *master = slave->master; |
1882 uint16_t rec_index; |
1904 uint16_t rec_index; |
1883 uint8_t *data, mbox_prot, rec_subindex; |
1905 uint8_t *data, mbox_prot, rec_subindex; |
1884 size_t rec_size, data_size; |
1906 size_t rec_size, data_size; |
1885 ec_sdo_request_t *request = fsm->request; |
1907 ec_sdo_request_t *request = fsm->request; |
1886 unsigned int expedited, size_specified; |
1908 unsigned int expedited, size_specified; |
1887 int ret; |
1909 int ret; |
1888 |
1910 |
1889 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1911 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
1890 return; // FIXME: request again? |
1912 return; // FIXME: request again? |
1891 |
1913 |
1892 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1914 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
1893 request->errno = EIO; |
1915 request->errno = EIO; |
1894 fsm->state = ec_fsm_coe_error; |
1916 fsm->state = ec_fsm_coe_error; |
1895 EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" |
1917 EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" |
1896 " datagram: "); |
1918 " datagram: "); |
1897 ec_datagram_print_state(datagram); |
1919 ec_datagram_print_state(datagram); |
1898 return; |
1920 return; |
1899 } |
1921 } |
1900 |
1922 |
1901 if (datagram->working_counter != 1) { |
1923 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
1902 request->errno = EIO; |
1924 request->errno = EIO; |
1903 fsm->state = ec_fsm_coe_error; |
1925 fsm->state = ec_fsm_coe_error; |
1904 EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); |
1926 EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); |
1905 ec_datagram_print_wc_error(datagram); |
1927 ec_datagram_print_wc_error(datagram); |
1906 return; |
1928 return; |
1907 } |
1929 } |
1908 |
1930 |
1909 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1931 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
1910 if (IS_ERR(data)) { |
1932 if (IS_ERR(data)) { |
1911 request->errno = PTR_ERR(data); |
1933 request->errno = PTR_ERR(data); |
1912 fsm->state = ec_fsm_coe_error; |
1934 fsm->state = ec_fsm_coe_error; |
1913 return; |
1935 return; |
1914 } |
1936 } |
1977 " (0x%04X:%02X, requested: 0x%04X:%02X).\n", |
1999 " (0x%04X:%02X, requested: 0x%04X:%02X).\n", |
1978 rec_index, rec_subindex, request->index, request->subindex); |
2000 rec_index, rec_subindex, request->index, request->subindex); |
1979 ec_print_data(data, rec_size); |
2001 ec_print_data(data, rec_size); |
1980 |
2002 |
1981 // check for CoE response again |
2003 // check for CoE response again |
1982 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2004 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
1983 fsm->retries = EC_FSM_RETRIES; |
2005 fsm->retries = EC_FSM_RETRIES; |
1984 fsm->state = ec_fsm_coe_up_check; |
2006 fsm->state = ec_fsm_coe_up_check; |
1985 return; |
2007 return; |
1986 } |
2008 } |
1987 |
2009 |
2073 \todo Timeout behavior |
2095 \todo Timeout behavior |
2074 */ |
2096 */ |
2075 |
2097 |
2076 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
2098 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
2077 { |
2099 { |
2078 ec_datagram_t *datagram = fsm->datagram; |
2100 ec_mailbox_t *mbox = fsm->mbox; |
|
2101 ec_datagram_t *datagram = mbox->datagram; |
2079 ec_slave_t *slave = fsm->slave; |
2102 ec_slave_t *slave = fsm->slave; |
2080 |
2103 |
2081 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2104 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
2082 return; // FIXME: check for response first? |
2105 return; // FIXME: check for response first? |
2083 |
2106 |
2084 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2107 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
2085 fsm->request->errno = EIO; |
2108 fsm->request->errno = EIO; |
2086 fsm->state = ec_fsm_coe_error; |
2109 fsm->state = ec_fsm_coe_error; |
2087 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2110 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2088 " request datagram: "); |
2111 " request datagram: "); |
2089 ec_datagram_print_state(datagram); |
2112 ec_datagram_print_state(datagram); |
2090 return; |
2113 return; |
2091 } |
2114 } |
2092 |
2115 |
2093 if (datagram->working_counter != 1) { |
2116 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
2094 fsm->request->errno = EIO; |
2117 fsm->request->errno = EIO; |
2095 fsm->state = ec_fsm_coe_error; |
2118 fsm->state = ec_fsm_coe_error; |
2096 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2119 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2097 " request failed: "); |
2120 " request failed: "); |
2098 ec_datagram_print_wc_error(datagram); |
2121 ec_datagram_print_wc_error(datagram); |
2099 return; |
2122 return; |
2100 } |
2123 } |
2101 |
2124 |
2102 fsm->jiffies_start = datagram->jiffies_sent; |
2125 fsm->jiffies_start = datagram->jiffies_sent; |
2103 |
2126 |
2104 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2127 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
2105 fsm->retries = EC_FSM_RETRIES; |
2128 fsm->retries = EC_FSM_RETRIES; |
2106 fsm->state = ec_fsm_coe_up_seg_check; |
2129 fsm->state = ec_fsm_coe_up_seg_check; |
2107 } |
2130 } |
2108 |
2131 |
2109 /*****************************************************************************/ |
2132 /*****************************************************************************/ |
2112 CoE state: UP CHECK. |
2135 CoE state: UP CHECK. |
2113 */ |
2136 */ |
2114 |
2137 |
2115 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
2138 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
2116 { |
2139 { |
2117 ec_datagram_t *datagram = fsm->datagram; |
2140 ec_mailbox_t *mbox = fsm->mbox; |
|
2141 ec_datagram_t *datagram = mbox->datagram; |
2118 ec_slave_t *slave = fsm->slave; |
2142 ec_slave_t *slave = fsm->slave; |
2119 |
2143 |
2120 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2144 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
2121 return; |
2145 return; |
2122 |
2146 |
2123 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2147 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
2124 fsm->request->errno = EIO; |
2148 fsm->request->errno = EIO; |
2125 fsm->state = ec_fsm_coe_error; |
2149 fsm->state = ec_fsm_coe_error; |
2126 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
2150 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
2127 " datagram: "); |
2151 " datagram: "); |
2128 ec_datagram_print_state(datagram); |
2152 ec_datagram_print_state(datagram); |
2129 return; |
2153 return; |
2130 } |
2154 } |
2131 |
2155 |
2132 if (datagram->working_counter != 1) { |
2156 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
2133 fsm->request->errno = EIO; |
2157 fsm->request->errno = EIO; |
2134 fsm->state = ec_fsm_coe_error; |
2158 fsm->state = ec_fsm_coe_error; |
2135 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" |
2159 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" |
2136 " failed: "); |
2160 " failed: "); |
2137 ec_datagram_print_wc_error(datagram); |
2161 ec_datagram_print_wc_error(datagram); |
2138 return; |
2162 return; |
2139 } |
2163 } |
2140 |
2164 |
2141 if (!ec_slave_mbox_check(datagram)) { |
2165 if (!ec_slave_mbox_check(mbox)) { |
2142 unsigned long diff_ms = |
2166 unsigned long diff_ms = |
2143 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
2167 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
2144 if (diff_ms >= fsm->request->response_timeout) { |
2168 if (diff_ms >= fsm->request->response_timeout) { |
2145 fsm->request->errno = EIO; |
2169 fsm->request->errno = EIO; |
2146 fsm->state = ec_fsm_coe_error; |
2170 fsm->state = ec_fsm_coe_error; |
2147 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" |
2171 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" |
2148 " segment response.\n"); |
2172 " segment response.\n"); |
2149 return; |
2173 return; |
2150 } |
2174 } |
2151 |
2175 |
2152 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2176 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
2153 fsm->retries = EC_FSM_RETRIES; |
2177 fsm->retries = EC_FSM_RETRIES; |
2154 return; |
2178 return; |
2155 } |
2179 } |
2156 |
2180 |
2157 // Fetch response |
2181 // Fetch response |
2158 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
2182 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
2159 fsm->retries = EC_FSM_RETRIES; |
2183 fsm->retries = EC_FSM_RETRIES; |
2160 fsm->state = ec_fsm_coe_up_seg_response; |
2184 fsm->state = ec_fsm_coe_up_seg_response; |
2161 } |
2185 } |
2162 |
2186 |
2163 /*****************************************************************************/ |
2187 /*****************************************************************************/ |
2167 \todo Timeout behavior |
2191 \todo Timeout behavior |
2168 */ |
2192 */ |
2169 |
2193 |
2170 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
2194 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
2171 { |
2195 { |
2172 ec_datagram_t *datagram = fsm->datagram; |
2196 ec_mailbox_t *mbox = fsm->mbox; |
|
2197 ec_datagram_t *datagram = mbox->datagram; |
2173 ec_slave_t *slave = fsm->slave; |
2198 ec_slave_t *slave = fsm->slave; |
2174 ec_master_t *master = slave->master; |
2199 ec_master_t *master = slave->master; |
2175 uint8_t *data, mbox_prot; |
2200 uint8_t *data, mbox_prot; |
2176 size_t rec_size, data_size; |
2201 size_t rec_size, data_size; |
2177 ec_sdo_request_t *request = fsm->request; |
2202 ec_sdo_request_t *request = fsm->request; |
2178 unsigned int last_segment; |
2203 unsigned int last_segment; |
2179 |
2204 |
2180 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2205 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
2181 return; // FIXME: request again? |
2206 return; // FIXME: request again? |
2182 |
2207 |
2183 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2208 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
2184 request->errno = EIO; |
2209 request->errno = EIO; |
2185 fsm->state = ec_fsm_coe_error; |
2210 fsm->state = ec_fsm_coe_error; |
2186 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2211 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2187 " response datagram: "); |
2212 " response datagram: "); |
2188 ec_datagram_print_state(datagram); |
2213 ec_datagram_print_state(datagram); |
2189 return; |
2214 return; |
2190 } |
2215 } |
2191 |
2216 |
2192 if (datagram->working_counter != 1) { |
2217 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
2193 request->errno = EIO; |
2218 request->errno = EIO; |
2194 fsm->state = ec_fsm_coe_error; |
2219 fsm->state = ec_fsm_coe_error; |
2195 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2220 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2196 " response failed: "); |
2221 " response failed: "); |
2197 ec_datagram_print_wc_error(datagram); |
2222 ec_datagram_print_wc_error(datagram); |
2198 return; |
2223 return; |
2199 } |
2224 } |
2200 |
2225 |
2201 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
2226 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
2202 if (IS_ERR(data)) { |
2227 if (IS_ERR(data)) { |
2203 request->errno = PTR_ERR(data); |
2228 request->errno = PTR_ERR(data); |
2204 fsm->state = ec_fsm_coe_error; |
2229 fsm->state = ec_fsm_coe_error; |
2205 return; |
2230 return; |
2206 } |
2231 } |
2251 if (fsm->slave->master->debug_level) { |
2276 if (fsm->slave->master->debug_level) { |
2252 EC_SLAVE_DBG(slave, 1, "Invalid SDO upload segment response!\n"); |
2277 EC_SLAVE_DBG(slave, 1, "Invalid SDO upload segment response!\n"); |
2253 ec_print_data(data, rec_size); |
2278 ec_print_data(data, rec_size); |
2254 } |
2279 } |
2255 // check for CoE response again |
2280 // check for CoE response again |
2256 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2281 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
2257 fsm->retries = EC_FSM_RETRIES; |
2282 fsm->retries = EC_FSM_RETRIES; |
2258 fsm->state = ec_fsm_coe_up_seg_check; |
2283 fsm->state = ec_fsm_coe_up_seg_check; |
2259 return; |
2284 return; |
2260 } |
2285 } |
2261 |
2286 |