Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_slave.c
changeset 635 d304ef4af542
parent 630 1b755b7342eb
child 637 d5d04c868e0e
equal deleted inserted replaced
634:ae5fb70b359f 635:d304ef4af542 
    40 
    40 
    41 #include "globals.h"
 
    41 #include "globals.h"
 
    42 #include "master.h"
 
    42 #include "master.h"
 
    43 #include "mailbox.h"
 
    43 #include "mailbox.h"
 
    44 #include "fsm_slave.h"
 
    44 #include "fsm_slave.h"
 
       
    45 #include "fsm_mapping.h"
 
    45 
    46 
    46 /*****************************************************************************/
 
    47 /*****************************************************************************/
 
    47 
    48 
    48 void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);
 
    49 void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);
 
    49 void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);
 
    50 void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);
 
    59 void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *);
 
    60 void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *);
 
    60 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);
 
    61 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);
 
    61 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *);
 
    62 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *);
 
    62 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);
 
    63 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);
 
    63 void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *);
 
    64 void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *);
 
       
    65 void ec_fsm_slave_conf_state_mapconf(ec_fsm_slave_t *);
 
    64 void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *);
 
    66 void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *);
 
    65 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);
 
    67 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);
 
    66 
    68 
    67 void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *);
 
    69 void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *);
 
    68 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);
 
    70 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);
 
    69 void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *);
 
    71 void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *);
 
    70 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);
 
    72 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);
 
    71 void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *);
 
    73 void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *);
 
       
    74 void ec_fsm_slave_conf_enter_mapconf(ec_fsm_slave_t *);
 
    72 void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *);
 
    75 void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *);
 
    73 
    76 
    74 void ec_fsm_slave_state_end(ec_fsm_slave_t *);
 
    77 void ec_fsm_slave_state_end(ec_fsm_slave_t *);
 
    75 void ec_fsm_slave_state_error(ec_fsm_slave_t *);
 
    78 void ec_fsm_slave_state_error(ec_fsm_slave_t *);
 
    76 
    79 
    88 
    91 
    89     // init sub state machines
 
    92     // init sub state machines
 
    90     ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);
 
    93     ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);
 
    91     ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
 
    94     ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
 
    92     ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
 
    95     ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
 
       
    96     ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe);
 
    93 }
 
    97 }
 
    94 
    98 
    95 /*****************************************************************************/
 
    99 /*****************************************************************************/
 
    96 
   100 
    97 /**
 
   101 /**
 
   102 {
 
   106 {
 
   103     // clear sub state machines
 
   107     // clear sub state machines
 
   104     ec_fsm_sii_clear(&fsm->fsm_sii);
 
   108     ec_fsm_sii_clear(&fsm->fsm_sii);
 
   105     ec_fsm_change_clear(&fsm->fsm_change);
 
   109     ec_fsm_change_clear(&fsm->fsm_change);
 
   106     ec_fsm_coe_clear(&fsm->fsm_coe);
 
   110     ec_fsm_coe_clear(&fsm->fsm_coe);
 
       
   111     ec_fsm_mapping_clear(&fsm->fsm_map);
 
   107 }
 
   112 }
 
   108 
   113 
   109 /*****************************************************************************/
 
   114 /*****************************************************************************/
 
   110 
   115 
   111 /**
 
   116 /**
 
   784                    slave->ring_position);
 
   789                    slave->ring_position);
 
   785         }
 
   790         }
 
   786         return;
 
   791         return;
 
   787     }
 
   792     }
 
   788 
   793 
       
   794     ec_fsm_slave_conf_enter_sdoconf(fsm);
 
       
   795 }
 
       
   796 
       
   797 /*****************************************************************************/
 
       
   798 
       
   799 /**
 
       
   800  */
 
       
   801 
       
   802 void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */)
 
       
   803 {
 
       
   804     ec_slave_t *slave = fsm->slave;
 
       
   805 
       
   806     // No CoE configuration to be applied?
 
       
   807     if (list_empty(&slave->sdo_confs)) { // skip SDO configuration
 
       
   808         ec_fsm_slave_conf_enter_mapconf(fsm);
 
       
   809         return;
 
       
   810     }
 
       
   811 
       
   812     // start SDO configuration
 
       
   813     fsm->state = ec_fsm_slave_conf_state_sdoconf;
 
       
   814     fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list);
 
       
   815     ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
 
       
   816     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
       
   817 }
 
       
   818 
       
   819 /*****************************************************************************/
 
       
   820 
       
   821 /**
 
       
   822    Slave configuration state: SDOCONF.
 
       
   823 */
 
       
   824 
       
   825 void ec_fsm_slave_conf_state_sdoconf(
 
       
   826         ec_fsm_slave_t *fsm /**< slave state machine */
 
       
   827         )
 
       
   828 {
 
       
   829     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
       
   830 
       
   831     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
       
   832         EC_ERR("SDO configuration failed for slave %u.\n",
 
       
   833                 fsm->slave->ring_position);
 
       
   834         fsm->slave->error_flag = 1;
 
       
   835         fsm->state = ec_fsm_slave_state_error;
 
       
   836         return;
 
       
   837     }
 
       
   838 
       
   839     // Another SDO to configure?
 
       
   840     if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) {
 
       
   841         fsm->sdodata = list_entry(fsm->sdodata->list.next,
 
       
   842                                   ec_sdo_data_t, list);
 
       
   843         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
 
       
   844         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
       
   845         return;
 
       
   846     }
 
       
   847 
       
   848     // All SDOs are now configured.
 
       
   849     ec_fsm_slave_conf_enter_mapconf(fsm);
 
       
   850 }
 
       
   851 
       
   852 /*****************************************************************************/
 
       
   853 
       
   854 /**
 
       
   855  */
 
       
   856 
       
   857 void ec_fsm_slave_conf_enter_mapconf(
 
       
   858         ec_fsm_slave_t *fsm /**< slave state machine */
 
       
   859         )
 
       
   860 {
 
       
   861     ec_slave_t *slave = fsm->slave;
 
       
   862 
       
   863     if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) {
 
       
   864         // Slave does not support CoE: no configuration of PDO mapping.
 
       
   865         ec_fsm_slave_conf_enter_pdo_sync(fsm);
 
       
   866         return;
 
       
   867     }
 
       
   868 
       
   869     // start configuring PDO mapping
 
       
   870     fsm->state = ec_fsm_slave_conf_state_mapconf;
 
       
   871     ec_fsm_mapping_start(&fsm->fsm_map, fsm->slave);
 
       
   872     ec_fsm_mapping_exec(&fsm->fsm_map); // execute immediately
 
       
   873 }
 
       
   874 
       
   875 /*****************************************************************************/
 
       
   876 
       
   877 /**
 
       
   878    Slave configuration state: MAPCONF.
 
       
   879 */
 
       
   880 
       
   881 void ec_fsm_slave_conf_state_mapconf(
 
       
   882         ec_fsm_slave_t *fsm /**< slave state machine */
 
       
   883         )
 
       
   884 {
 
       
   885     if (ec_fsm_mapping_exec(&fsm->fsm_map)) return;
 
       
   886 
       
   887     if (!ec_fsm_mapping_success(&fsm->fsm_map)) {
 
       
   888         EC_ERR("PDO mapping configuration failed for slave %u.\n",
 
       
   889                 fsm->slave->ring_position);
 
       
   890         fsm->slave->error_flag = 1;
 
       
   891         fsm->state = ec_fsm_slave_state_error;
 
       
   892         return;
 
       
   893     }
 
       
   894 
   789     ec_fsm_slave_conf_enter_pdo_sync(fsm);
 
   895     ec_fsm_slave_conf_enter_pdo_sync(fsm);
 
   790 }
 
   896 }
 
   791 
   897 
   792 /*****************************************************************************/
 
   898 /*****************************************************************************/
 
   793 
   899 
   823 }
 
   929 }
 
   824 
   930 
   825 /*****************************************************************************/
 
   931 /*****************************************************************************/
 
   826 
   932 
   827 /**
 
   933 /**
 
   828    Slave configuration state: SYNC2.
 
   934  */
 
   829 */
 
       
   830 
   935 
   831 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *fsm /**< slave state machine */)
 
   936 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *fsm /**< slave state machine */)
 
   832 {
 
   937 {
 
   833     ec_datagram_t *datagram = fsm->datagram;
 
   938     ec_datagram_t *datagram = fsm->datagram;
 
   834     ec_slave_t *slave = fsm->slave;
 
   939     ec_slave_t *slave = fsm->slave;
 
   868     ec_master_t *master = slave->master;
 
   973     ec_master_t *master = slave->master;
 
   869     ec_datagram_t *datagram = fsm->datagram;
 
   974     ec_datagram_t *datagram = fsm->datagram;
 
   870     unsigned int j;
 
   975     unsigned int j;
 
   871 
   976 
   872     if (!slave->base_fmmu_count) { // skip FMMU configuration
 
   977     if (!slave->base_fmmu_count) { // skip FMMU configuration
 
   873         ec_fsm_slave_conf_enter_sdoconf(fsm);
 
   978         ec_fsm_slave_conf_enter_saveop(fsm);
 
   874         return;
 
   979         return;
 
   875     }
 
   980     }
 
   876 
   981 
   877     // configure FMMUs
 
   982     // configure FMMUs
 
   878     ec_datagram_npwr(datagram, slave->station_address,
 
   983     ec_datagram_npwr(datagram, slave->station_address,
 
   917         EC_ERR("Failed to set FMMUs - slave %i did not respond.\n",
 
  1022         EC_ERR("Failed to set FMMUs - slave %i did not respond.\n",
 
   918                slave->ring_position);
 
  1023                slave->ring_position);
 
   919         return;
 
  1024         return;
 
   920     }
 
  1025     }
 
   921 
  1026 
   922     ec_fsm_slave_conf_enter_sdoconf(fsm);
 
       
   923 }
 
       
   924 
       
   925 /*****************************************************************************/
 
       
   926 
       
   927 /**
 
       
   928  */
 
       
   929 
       
   930 void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */)
 
       
   931 {
 
       
   932     ec_slave_t *slave = fsm->slave;
 
       
   933 
       
   934     // No CoE configuration to be applied? Jump to SAVEOP state.
 
       
   935     if (list_empty(&slave->sdo_confs)) { // skip SDO configuration
 
       
   936         ec_fsm_slave_conf_enter_saveop(fsm);
 
       
   937         return;
 
       
   938     }
 
       
   939 
       
   940     // start SDO configuration
 
       
   941     fsm->state = ec_fsm_slave_conf_state_sdoconf;
 
       
   942     fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list);
 
       
   943     ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
 
       
   944     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
       
   945 }
 
       
   946 
       
   947 /*****************************************************************************/
 
       
   948 
       
   949 /**
 
       
   950    Slave configuration state: SDOCONF.
 
       
   951 */
 
       
   952 
       
   953 void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */)
 
       
   954 {
 
       
   955     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
       
   956 
       
   957     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
       
   958         fsm->slave->error_flag = 1;
 
       
   959         fsm->state = ec_fsm_slave_state_error;
 
       
   960         return;
 
       
   961     }
 
       
   962 
       
   963     // Another SDO to configure?
 
       
   964     if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) {
 
       
   965         fsm->sdodata = list_entry(fsm->sdodata->list.next,
 
       
   966                                   ec_sdo_data_t, list);
 
       
   967         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
 
       
   968         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
 
       
   969         return;
 
       
   970     }
 
       
   971 
       
   972     // All SDOs are now configured.
 
       
   973 
       
   974     // set state to SAVEOP
 
       
   975     ec_fsm_slave_conf_enter_saveop(fsm);
 
  1027     ec_fsm_slave_conf_enter_saveop(fsm);
 
   976 }
 
  1028 }
 
   977 
  1029 
   978 /*****************************************************************************/
 
  1030 /*****************************************************************************/
 
   979 
  1031
etherlabmaster