677 fsm->request = list_entry(fsm->slave->config->sdo_configs.next, |
677 fsm->request = list_entry(fsm->slave->config->sdo_configs.next, |
678 ec_sdo_request_t, list); |
678 ec_sdo_request_t, list); |
679 ec_sdo_request_copy(&fsm->request_copy, fsm->request); |
679 ec_sdo_request_copy(&fsm->request_copy, fsm->request); |
680 ecrt_sdo_request_write(&fsm->request_copy); |
680 ecrt_sdo_request_write(&fsm->request_copy); |
681 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); |
681 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); |
682 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
682 ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately |
683 } |
683 } |
684 |
684 |
685 /*****************************************************************************/ |
685 /*****************************************************************************/ |
686 |
686 |
687 /** Slave configuration state: SDO_CONF. |
687 /** Slave configuration state: SDO_CONF. |
688 */ |
688 */ |
689 void ec_fsm_slave_config_state_sdo_conf( |
689 void ec_fsm_slave_config_state_sdo_conf( |
690 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
690 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
691 ) |
691 ) |
692 { |
692 { |
693 if (ec_fsm_coe_exec(fsm->fsm_coe)) { |
693 if (ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram)) { |
694 return; |
694 return; |
695 } |
695 } |
696 |
696 |
697 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
697 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
698 EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); |
698 EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); |
711 fsm->request = list_entry(fsm->request->list.next, |
711 fsm->request = list_entry(fsm->request->list.next, |
712 ec_sdo_request_t, list); |
712 ec_sdo_request_t, list); |
713 ec_sdo_request_copy(&fsm->request_copy, fsm->request); |
713 ec_sdo_request_copy(&fsm->request_copy, fsm->request); |
714 ecrt_sdo_request_write(&fsm->request_copy); |
714 ecrt_sdo_request_write(&fsm->request_copy); |
715 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); |
715 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); |
716 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
716 ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately |
717 return; |
717 return; |
718 } |
718 } |
719 |
719 |
720 // All SDOs are now configured. |
720 // All SDOs are now configured. |
721 ec_fsm_slave_config_enter_soe_conf_preop(fsm); |
721 ec_fsm_slave_config_enter_soe_conf_preop(fsm); |
744 fsm->soe_request = req; |
744 fsm->soe_request = req; |
745 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
745 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
746 ec_soe_request_write(&fsm->soe_request_copy); |
746 ec_soe_request_write(&fsm->soe_request_copy); |
747 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
747 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
748 &fsm->soe_request_copy); |
748 &fsm->soe_request_copy); |
749 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
749 ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram); |
750 return; |
750 return; |
751 } |
751 } |
752 } |
752 } |
753 |
753 |
754 // No SoE configuration to be applied in PREOP |
754 // No SoE configuration to be applied in PREOP |
763 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
763 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
764 ) |
764 ) |
765 { |
765 { |
766 ec_slave_t *slave = fsm->slave; |
766 ec_slave_t *slave = fsm->slave; |
767 |
767 |
768 if (ec_fsm_soe_exec(fsm->fsm_soe)) { |
768 if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) { |
769 return; |
769 return; |
770 } |
770 } |
771 |
771 |
772 if (!ec_fsm_soe_success(fsm->fsm_soe)) { |
772 if (!ec_fsm_soe_success(fsm->fsm_soe)) { |
773 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
773 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
788 if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { |
788 if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) { |
789 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
789 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
790 ec_soe_request_write(&fsm->soe_request_copy); |
790 ec_soe_request_write(&fsm->soe_request_copy); |
791 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
791 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
792 &fsm->soe_request_copy); |
792 &fsm->soe_request_copy); |
793 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
793 ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram); |
794 return; |
794 return; |
795 } |
795 } |
796 } |
796 } |
797 |
797 |
798 // All PREOP IDNs are now configured. |
798 // All PREOP IDNs are now configured. |
821 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
821 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
822 ) |
822 ) |
823 { |
823 { |
824 // TODO check for config here |
824 // TODO check for config here |
825 |
825 |
826 if (ec_fsm_pdo_exec(fsm->fsm_pdo)) |
826 if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) { |
827 return; |
827 return; |
|
828 } |
828 |
829 |
829 if (!fsm->slave->config) { // config removed in the meantime |
830 if (!fsm->slave->config) { // config removed in the meantime |
830 ec_fsm_slave_config_reconfigure(fsm); |
831 ec_fsm_slave_config_reconfigure(fsm); |
831 return; |
832 return; |
832 } |
833 } |
1468 fsm->soe_request = req; |
1469 fsm->soe_request = req; |
1469 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1470 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1470 ec_soe_request_write(&fsm->soe_request_copy); |
1471 ec_soe_request_write(&fsm->soe_request_copy); |
1471 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
1472 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
1472 &fsm->soe_request_copy); |
1473 &fsm->soe_request_copy); |
1473 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
1474 ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram); |
1474 return; |
1475 return; |
1475 } |
1476 } |
1476 } |
1477 } |
1477 |
1478 |
1478 // No SoE configuration to be applied in SAFEOP |
1479 // No SoE configuration to be applied in SAFEOP |
1487 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1488 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1488 ) |
1489 ) |
1489 { |
1490 { |
1490 ec_slave_t *slave = fsm->slave; |
1491 ec_slave_t *slave = fsm->slave; |
1491 |
1492 |
1492 if (ec_fsm_soe_exec(fsm->fsm_soe)) { |
1493 if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) { |
1493 return; |
1494 return; |
1494 } |
1495 } |
1495 |
1496 |
1496 if (!ec_fsm_soe_success(fsm->fsm_soe)) { |
1497 if (!ec_fsm_soe_success(fsm->fsm_soe)) { |
1497 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
1498 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
1512 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
1513 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
1513 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1514 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1514 ec_soe_request_write(&fsm->soe_request_copy); |
1515 ec_soe_request_write(&fsm->soe_request_copy); |
1515 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
1516 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
1516 &fsm->soe_request_copy); |
1517 &fsm->soe_request_copy); |
1517 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
1518 ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram); |
1518 return; |
1519 return; |
1519 } |
1520 } |
1520 } |
1521 } |
1521 |
1522 |
1522 // All SAFEOP IDNs are now configured. |
1523 // All SAFEOP IDNs are now configured. |