Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_slave_config.c
changeset 1396 9d04cc08f40f
parent 1392 8fcc1d0987c1
child 1404 b7ea043d5081
equal deleted inserted replaced
1395:e9fe40c458cc 1396:9d04cc08f40f 
    50 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);
 
    50 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);
 
    51 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
 
    51 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
 
    52 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
 
    52 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
 
    53 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);
 
    53 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);
 
    54 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);
 
    54 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);
 
       
    55 void ec_fsm_slave_config_state_dc_read(ec_fsm_slave_config_t *);
 
       
    56 void ec_fsm_slave_config_state_dc_offset(ec_fsm_slave_config_t *);
 
    55 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);
 
    57 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);
 
    56 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);
 
    58 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);
 
    57 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);
 
    59 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);
 
    58 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);
 
    60 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);
 
    59 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);
 
    61 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);
 
    64 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);
 
    66 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);
 
    65 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);
 
    67 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);
 
    66 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);
 
    68 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);
 
    67 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);
 
    69 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);
 
    68 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);
 
    70 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);
 
    69 void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);
 
    71 void ec_fsm_slave_config_enter_dc_read(ec_fsm_slave_config_t *);
 
    70 void ec_fsm_slave_config_enter_dc_assign(ec_fsm_slave_config_t *);
 
       
    71 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);
 
    72 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);
 
    72 
    73 
    73 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);
 
    74 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);
 
    74 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);
 
    75 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);
 
    75 
    76 
   809                 slave->config->used_fmmus);
 
   810                 slave->config->used_fmmus);
 
   810         return;
 
   811         return;
 
   811     }
 
   812     }
 
   812 
   813 
   813     if (!slave->base_fmmu_count) { // skip FMMU configuration
 
   814     if (!slave->base_fmmu_count) { // skip FMMU configuration
 
   814         ec_fsm_slave_config_enter_dc_cycle(fsm);
 
   815         ec_fsm_slave_config_enter_dc_read(fsm);
 
   815         return;
 
   816         return;
 
   816     }
 
   817     }
 
   817 
   818 
   818     // configure FMMUs
 
   819     // configure FMMUs
 
   819     ec_datagram_fpwr(datagram, slave->station_address,
 
   820     ec_datagram_fpwr(datagram, slave->station_address,
 
   865                slave->ring_position);
 
   866                slave->ring_position);
 
   866         ec_datagram_print_wc_error(datagram);
 
   867         ec_datagram_print_wc_error(datagram);
 
   867         return;
 
   868         return;
 
   868     }
 
   869     }
 
   869 
   870 
   870 	ec_fsm_slave_config_enter_dc_cycle(fsm);
 
   871     ec_fsm_slave_config_enter_dc_read(fsm);
 
   871 }
 
   872 }
 
   872 
   873 
   873 /*****************************************************************************/
 
   874 /*****************************************************************************/
 
   874 
   875 
   875 /** Check for DCs to be configured.
 
   876 /** Check for DCs to be configured.
 
   876  */
 
   877  */
 
   877 void ec_fsm_slave_config_enter_dc_cycle(
 
   878 void ec_fsm_slave_config_enter_dc_read(
 
   878         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
   879         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
   879         )
 
   880         )
 
   880 {
 
   881 {
 
   881     ec_datagram_t *datagram = fsm->datagram;
 
   882     ec_slave_t *slave = fsm->slave;
 
   882     ec_slave_t *slave = fsm->slave;
 
   883 
   883     ec_slave_config_t *config = slave->config;
 
   884     if (slave->base_dc_supported) {
 
       
   885         // read DC system time and system time offset
 
       
   886         ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0910, 24);
 
       
   887         fsm->retries = EC_FSM_RETRIES;
 
       
   888         fsm->state = ec_fsm_slave_config_state_dc_read;
 
       
   889     } else {
 
       
   890         ec_fsm_slave_config_enter_safeop(fsm);
 
       
   891     }
 
       
   892 }
 
       
   893 
       
   894 /*****************************************************************************/
 
       
   895 
       
   896 /** Slave configuration state: DC READ.
 
       
   897  */
 
       
   898 void ec_fsm_slave_config_state_dc_read(
 
       
   899         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
       
   900         )
 
       
   901 {
 
       
   902     ec_datagram_t *datagram = fsm->datagram;
 
       
   903     ec_slave_t *slave = fsm->slave;
 
       
   904     u64 system_time, old_offset, new_offset;
 
       
   905 
       
   906     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
       
   907         return;
 
       
   908 
       
   909     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
       
   910         fsm->state = ec_fsm_slave_config_state_error;
 
       
   911         EC_ERR("Failed to receive DC times datagram for slave %u"
 
       
   912                 " (datagram state %u).\n",
 
       
   913                 slave->ring_position, datagram->state);
 
       
   914         return;
 
       
   915     }
 
       
   916 
       
   917     if (datagram->working_counter != 1) {
 
       
   918         slave->error_flag = 1;
 
       
   919         fsm->state = ec_fsm_slave_config_state_error;
 
       
   920         EC_ERR("Failed to get DC times of slave %u: ",
 
       
   921                 slave->ring_position);
 
       
   922         ec_datagram_print_wc_error(datagram);
 
       
   923         return;
 
       
   924     }
 
       
   925 
       
   926     system_time = EC_READ_U64(datagram->data);
 
       
   927     old_offset = EC_READ_U64(datagram->data + 16);
 
       
   928     new_offset = slave->master->app_time - system_time + old_offset;
 
       
   929 
       
   930     if (slave->master->debug_level)
 
       
   931         EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, "
 
       
   932                 "app_time=%llu, new_offset=%llu\n",
 
       
   933                 slave->ring_position, system_time, old_offset,
 
       
   934                 slave->master->app_time, new_offset);
 
       
   935 
       
   936     // set DC system time offset
 
       
   937     ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 8);
 
       
   938     EC_WRITE_U64(datagram->data, new_offset);
 
       
   939     fsm->retries = EC_FSM_RETRIES;
 
       
   940     fsm->state = ec_fsm_slave_config_state_dc_offset;
 
       
   941 }
 
       
   942 
       
   943 /*****************************************************************************/
 
       
   944 
       
   945 /** Slave configuration state: DC OFFSET.
 
       
   946  */
 
       
   947 void ec_fsm_slave_config_state_dc_offset(
 
       
   948         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
       
   949         )
 
       
   950 {
 
       
   951     ec_datagram_t *datagram = fsm->datagram;
 
       
   952     ec_slave_t *slave = fsm->slave;
 
       
   953     ec_slave_config_t *config = slave->config; // FIXME
 
       
   954 
       
   955     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
       
   956         return;
 
       
   957 
       
   958     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
       
   959         fsm->state = ec_fsm_slave_config_state_error;
 
       
   960         EC_ERR("Failed to receive DC system time offset datagram for slave %u"
 
       
   961                 " (datagram state %u).\n",
 
       
   962                 slave->ring_position, datagram->state);
 
       
   963         return;
 
       
   964     }
 
       
   965 
       
   966     if (datagram->working_counter != 1) {
 
       
   967         slave->error_flag = 1;
 
       
   968         fsm->state = ec_fsm_slave_config_state_error;
 
       
   969         EC_ERR("Failed to set DC system time offset of slave %u: ",
 
       
   970                 slave->ring_position);
 
       
   971         ec_datagram_print_wc_error(datagram);
 
       
   972         return;
 
       
   973     }
 
   884 
   974 
   885     if (config->dc_assign_activate) {
 
   975     if (config->dc_assign_activate) {
 
   886         if (!slave->base_dc_supported) {
 
       
   887             EC_WARN("Attempt to enable synchronized mode for slave %u,"
 
       
   888                     " that seems not to support distributed clocks!\n",
 
       
   889                     slave->ring_position);
 
       
   890         }
 
       
   891 
       
   892         // set DC cycle times
 
   976         // set DC cycle times
 
   893         ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8);
 
   977         ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8);
 
   894         EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]);
 
   978         EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]);
 
   895         EC_WRITE_U32(datagram->data + 4, config->dc_sync_cycle_times[1]);
 
   979         EC_WRITE_U32(datagram->data + 4, config->dc_sync_cycle_times[1]);
 
   896         fsm->retries = EC_FSM_RETRIES;
 
   980         fsm->retries = EC_FSM_RETRIES;
 
   897         fsm->state = ec_fsm_slave_config_state_dc_cycle;
 
   981         fsm->state = ec_fsm_slave_config_state_dc_cycle;
 
   898     } else {
 
   982     } else {
 
   899         ec_fsm_slave_config_enter_dc_assign(fsm);
 
   983         ec_fsm_slave_config_enter_safeop(fsm);
 
   900     }
 
   984     }
 
   901 }
 
   985 }
 
   902 
   986 
   903 /*****************************************************************************/
 
   987 /*****************************************************************************/
 
   904 
   988 
   908         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
   992         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
   909         )
 
   993         )
 
   910 {
 
   994 {
 
   911     ec_datagram_t *datagram = fsm->datagram;
 
   995     ec_datagram_t *datagram = fsm->datagram;
 
   912     ec_slave_t *slave = fsm->slave;
 
   996     ec_slave_t *slave = fsm->slave;
 
       
   997     u64 start_time;
 
   913 
   998 
   914     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   999     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   915         return;
 
  1000         return;
 
   916 
  1001 
   917     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1002     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   930         ec_datagram_print_wc_error(datagram);
 
  1015         ec_datagram_print_wc_error(datagram);
 
   931         return;
 
  1016         return;
 
   932     }
 
  1017     }
 
   933 
  1018 
   934     // set DC start time
 
  1019     // set DC start time
 
       
  1020     start_time = slave->master->app_time + 1000000000; // now plus 1 s
 
       
  1021     if (slave->master->debug_level)
 
       
  1022         EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n",
 
       
  1023                 slave->ring_position, start_time);
 
       
  1024 
   935     ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8);
 
  1025     ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8);
 
   936     EC_WRITE_U64(datagram->data, 0x37E11D600ULL); // 15 s, FIXME
 
  1026     EC_WRITE_U64(datagram->data, start_time);
 
   937     fsm->retries = EC_FSM_RETRIES;
 
  1027     fsm->retries = EC_FSM_RETRIES;
 
   938     fsm->state = ec_fsm_slave_config_state_dc_start;
 
  1028     fsm->state = ec_fsm_slave_config_state_dc_start;
 
   939 }
 
  1029 }
 
   940 
  1030 
   941 /*****************************************************************************/
 
  1031 /*****************************************************************************/
 
   946         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
  1036         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
   947         )
 
  1037         )
 
   948 {
 
  1038 {
 
   949     ec_datagram_t *datagram = fsm->datagram;
 
  1039     ec_datagram_t *datagram = fsm->datagram;
 
   950     ec_slave_t *slave = fsm->slave;
 
  1040     ec_slave_t *slave = fsm->slave;
 
       
  1041     ec_slave_config_t *config = slave->config; // FIXME
 
   951 
  1042 
   952     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
  1043     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   953         return;
 
  1044         return;
 
   954 
  1045 
   955     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1046     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   966         EC_ERR("Failed to set DC start time of slave %u: ",
 
  1057         EC_ERR("Failed to set DC start time of slave %u: ",
 
   967                 slave->ring_position);
 
  1058                 slave->ring_position);
 
   968         ec_datagram_print_wc_error(datagram);
 
  1059         ec_datagram_print_wc_error(datagram);
 
   969         return;
 
  1060         return;
 
   970     }
 
  1061     }
 
   971 
       
   972     ec_fsm_slave_config_enter_dc_assign(fsm);
 
       
   973 }
 
       
   974 
       
   975 /*****************************************************************************/
 
       
   976 
       
   977 /** Set the DC AssignActivate word.
 
       
   978  */
 
       
   979 void ec_fsm_slave_config_enter_dc_assign(
 
       
   980         ec_fsm_slave_config_t *fsm /**< slave state machine */
 
       
   981         )
 
       
   982 {
 
       
   983     ec_datagram_t *datagram = fsm->datagram;
 
       
   984     ec_slave_t *slave = fsm->slave;
 
       
   985     ec_slave_config_t *config = slave->config;
 
       
   986 
  1062 
   987     // assign sync unit to EtherCAT or PDI
 
  1063     // assign sync unit to EtherCAT or PDI
 
   988     ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);
 
  1064     ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);
 
   989     EC_WRITE_U16(datagram->data, config->dc_assign_activate);
 
  1065     EC_WRITE_U16(datagram->data, config->dc_assign_activate);
 
   990     fsm->retries = EC_FSM_RETRIES;
 
  1066     fsm->retries = EC_FSM_RETRIES;
etherlabmaster