112 void ec_fsm_slave_config_init( |
112 void ec_fsm_slave_config_init( |
113 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
113 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
114 ec_datagram_t *datagram, /**< datagram structure to use */ |
114 ec_datagram_t *datagram, /**< datagram structure to use */ |
115 ec_fsm_change_t *fsm_change, /**< State change state machine to use. */ |
115 ec_fsm_change_t *fsm_change, /**< State change state machine to use. */ |
116 ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */ |
116 ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */ |
|
117 ec_fsm_soe_t *fsm_soe, /**< SoE state machine to use. */ |
117 ec_fsm_pdo_t *fsm_pdo /**< PDO configuration state machine to use. */ |
118 ec_fsm_pdo_t *fsm_pdo /**< PDO configuration state machine to use. */ |
118 ) |
119 ) |
119 { |
120 { |
120 ec_sdo_request_init(&fsm->request_copy); |
121 ec_sdo_request_init(&fsm->request_copy); |
121 ec_soe_request_init(&fsm->soe_request_copy); |
122 ec_soe_request_init(&fsm->soe_request_copy); |
122 |
123 |
123 fsm->datagram = datagram; |
124 fsm->datagram = datagram; |
124 fsm->fsm_change = fsm_change; |
125 fsm->fsm_change = fsm_change; |
125 fsm->fsm_coe = fsm_coe; |
126 fsm->fsm_coe = fsm_coe; |
|
127 fsm->fsm_soe = fsm_soe; |
126 fsm->fsm_pdo = fsm_pdo; |
128 fsm->fsm_pdo = fsm_pdo; |
127 } |
129 } |
128 |
130 |
129 /*****************************************************************************/ |
131 /*****************************************************************************/ |
130 |
132 |
686 */ |
688 */ |
687 void ec_fsm_slave_config_state_sdo_conf( |
689 void ec_fsm_slave_config_state_sdo_conf( |
688 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
690 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
689 ) |
691 ) |
690 { |
692 { |
691 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
693 if (ec_fsm_coe_exec(fsm->fsm_coe)) { |
|
694 return; |
|
695 } |
692 |
696 |
693 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
697 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
694 EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); |
698 EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); |
695 fsm->slave->error_flag = 1; |
699 fsm->slave->error_flag = 1; |
696 fsm->state = ec_fsm_slave_config_state_error; |
700 fsm->state = ec_fsm_slave_config_state_error; |
724 void ec_fsm_slave_config_enter_soe_conf_preop( |
728 void ec_fsm_slave_config_enter_soe_conf_preop( |
725 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
729 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
726 ) |
730 ) |
727 { |
731 { |
728 ec_slave_t *slave = fsm->slave; |
732 ec_slave_t *slave = fsm->slave; |
729 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
|
730 ec_soe_request_t *req; |
733 ec_soe_request_t *req; |
731 |
734 |
732 if (!slave->config) { |
735 if (!slave->config) { |
733 ec_fsm_slave_config_enter_pdo_sync(fsm); |
736 ec_fsm_slave_config_enter_pdo_sync(fsm); |
734 return; |
737 return; |
739 // start SoE configuration |
742 // start SoE configuration |
740 fsm->state = ec_fsm_slave_config_state_soe_conf_preop; |
743 fsm->state = ec_fsm_slave_config_state_soe_conf_preop; |
741 fsm->soe_request = req; |
744 fsm->soe_request = req; |
742 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
745 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
743 ec_soe_request_write(&fsm->soe_request_copy); |
746 ec_soe_request_write(&fsm->soe_request_copy); |
744 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
747 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
745 ec_fsm_soe_exec(fsm_soe); // execute immediately |
748 &fsm->soe_request_copy); |
746 ec_master_queue_external_datagram(slave->master, |
749 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
747 fsm_soe->datagram); |
|
748 return; |
750 return; |
749 } |
751 } |
750 } |
752 } |
751 |
753 |
752 // No SoE configuration to be applied in PREOP |
754 // No SoE configuration to be applied in PREOP |
760 void ec_fsm_slave_config_state_soe_conf_preop( |
762 void ec_fsm_slave_config_state_soe_conf_preop( |
761 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
763 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
762 ) |
764 ) |
763 { |
765 { |
764 ec_slave_t *slave = fsm->slave; |
766 ec_slave_t *slave = fsm->slave; |
765 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
767 |
766 |
768 if (ec_fsm_soe_exec(fsm->fsm_soe)) { |
767 if (ec_fsm_soe_exec(fsm_soe)) { |
769 return; |
768 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
770 } |
769 return; |
771 |
770 } |
772 if (!ec_fsm_soe_success(fsm->fsm_soe)) { |
771 |
|
772 if (!ec_fsm_soe_success(fsm_soe)) { |
|
773 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
773 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
774 fsm->slave->error_flag = 1; |
774 fsm->slave->error_flag = 1; |
775 fsm->state = ec_fsm_slave_config_state_error; |
775 fsm->state = ec_fsm_slave_config_state_error; |
776 return; |
776 return; |
777 } |
777 } |
786 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
786 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
787 ec_soe_request_t, list); |
787 ec_soe_request_t, list); |
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_soe, fsm->slave, &fsm->soe_request_copy); |
791 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
792 ec_fsm_soe_exec(fsm_soe); // execute immediately |
792 &fsm->soe_request_copy); |
793 ec_master_queue_external_datagram(slave->master, |
793 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
794 fsm_soe->datagram); |
|
795 return; |
794 return; |
796 } |
795 } |
797 } |
796 } |
798 |
797 |
799 // All PREOP IDNs are now configured. |
798 // All PREOP IDNs are now configured. |
1453 void ec_fsm_slave_config_enter_soe_conf_safeop( |
1452 void ec_fsm_slave_config_enter_soe_conf_safeop( |
1454 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1453 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1455 ) |
1454 ) |
1456 { |
1455 { |
1457 ec_slave_t *slave = fsm->slave; |
1456 ec_slave_t *slave = fsm->slave; |
1458 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
|
1459 ec_soe_request_t *req; |
1457 ec_soe_request_t *req; |
1460 |
1458 |
1461 if (!slave->config) { |
1459 if (!slave->config) { |
1462 ec_fsm_slave_config_enter_op(fsm); |
1460 ec_fsm_slave_config_enter_op(fsm); |
1463 return; |
1461 return; |
1468 // start SoE configuration |
1466 // start SoE configuration |
1469 fsm->state = ec_fsm_slave_config_state_soe_conf_safeop; |
1467 fsm->state = ec_fsm_slave_config_state_soe_conf_safeop; |
1470 fsm->soe_request = req; |
1468 fsm->soe_request = req; |
1471 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1469 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1472 ec_soe_request_write(&fsm->soe_request_copy); |
1470 ec_soe_request_write(&fsm->soe_request_copy); |
1473 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
1471 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
1474 ec_fsm_soe_exec(fsm_soe); // execute immediately |
1472 &fsm->soe_request_copy); |
1475 ec_master_queue_external_datagram(slave->master, |
1473 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
1476 fsm_soe->datagram); |
|
1477 return; |
1474 return; |
1478 } |
1475 } |
1479 } |
1476 } |
1480 |
1477 |
1481 // No SoE configuration to be applied in SAFEOP |
1478 // No SoE configuration to be applied in SAFEOP |
1489 void ec_fsm_slave_config_state_soe_conf_safeop( |
1486 void ec_fsm_slave_config_state_soe_conf_safeop( |
1490 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1487 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1491 ) |
1488 ) |
1492 { |
1489 { |
1493 ec_slave_t *slave = fsm->slave; |
1490 ec_slave_t *slave = fsm->slave; |
1494 ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe; |
1491 |
1495 |
1492 if (ec_fsm_soe_exec(fsm->fsm_soe)) { |
1496 if (ec_fsm_soe_exec(fsm_soe)) { |
1493 return; |
1497 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
1494 } |
1498 return; |
1495 |
1499 } |
1496 if (!ec_fsm_soe_success(fsm->fsm_soe)) { |
1500 |
|
1501 if (!ec_fsm_soe_success(fsm_soe)) { |
|
1502 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
1497 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
1503 fsm->slave->error_flag = 1; |
1498 fsm->slave->error_flag = 1; |
1504 fsm->state = ec_fsm_slave_config_state_error; |
1499 fsm->state = ec_fsm_slave_config_state_error; |
1505 return; |
1500 return; |
1506 } |
1501 } |
1515 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
1510 fsm->soe_request = list_entry(fsm->soe_request->list.next, |
1516 ec_soe_request_t, list); |
1511 ec_soe_request_t, list); |
1517 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
1512 if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) { |
1518 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1513 ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request); |
1519 ec_soe_request_write(&fsm->soe_request_copy); |
1514 ec_soe_request_write(&fsm->soe_request_copy); |
1520 ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy); |
1515 ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave, |
1521 ec_fsm_soe_exec(fsm_soe); // execute immediately |
1516 &fsm->soe_request_copy); |
1522 ec_master_queue_external_datagram(slave->master, |
1517 ec_fsm_soe_exec(fsm->fsm_soe); // execute immediately |
1523 fsm_soe->datagram); |
|
1524 return; |
1518 return; |
1525 } |
1519 } |
1526 } |
1520 } |
1527 |
1521 |
1528 // All SAFEOP IDNs are now configured. |
1522 // All SAFEOP IDNs are now configured. |