master/fsm_slave.c
changeset 814 a51f857b1b2d
parent 813 bfc3f1ab52de
child 815 002fe9ec778f
equal deleted inserted replaced
813:bfc3f1ab52de 814:a51f857b1b2d
   867 }
   867 }
   868 
   868 
   869 /*****************************************************************************/
   869 /*****************************************************************************/
   870 
   870 
   871 /**
   871 /**
   872  * Check for SDO configurations to be applied.
   872  * Check for Sdo configurations to be applied.
   873  */
   873  */
   874 
   874 
   875 void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *fsm /**< slave state machine */)
   875 void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *fsm /**< slave state machine */)
   876 {
   876 {
   877     ec_slave_t *slave = fsm->slave;
   877     ec_slave_t *slave = fsm->slave;
   881         ec_fsm_slave_conf_enter_safeop(fsm);
   881         ec_fsm_slave_conf_enter_safeop(fsm);
   882         return;
   882         return;
   883     }
   883     }
   884 
   884 
   885     // No CoE configuration to be applied?
   885     // No CoE configuration to be applied?
   886     if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration
   886     if (list_empty(&slave->config->sdo_configs)) { // skip Sdo configuration
   887         ec_fsm_slave_conf_enter_mapping(fsm);
   887         ec_fsm_slave_conf_enter_mapping(fsm);
   888         return;
   888         return;
   889     }
   889     }
   890 
   890 
   891     // start SDO configuration
   891     // start Sdo configuration
   892     fsm->state = ec_fsm_slave_conf_state_sdo_conf;
   892     fsm->state = ec_fsm_slave_conf_state_sdo_conf;
   893     fsm->sdodata =
   893     fsm->sdodata =
   894         list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list);
   894         list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list);
   895     ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
   895     ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
   896     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
   896     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
   907         )
   907         )
   908 {
   908 {
   909     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
   909     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
   910 
   910 
   911     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
   911     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
   912         EC_ERR("SDO configuration failed for slave %u.\n",
   912         EC_ERR("Sdo configuration failed for slave %u.\n",
   913                 fsm->slave->ring_position);
   913                 fsm->slave->ring_position);
   914         fsm->slave->error_flag = 1;
   914         fsm->slave->error_flag = 1;
   915         fsm->state = ec_fsm_slave_state_error;
   915         fsm->state = ec_fsm_slave_state_error;
   916         return;
   916         return;
   917     }
   917     }
   918 
   918 
   919     // Another SDO to configure?
   919     // Another Sdo to configure?
   920     if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) {
   920     if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) {
   921         fsm->sdodata =
   921         fsm->sdodata =
   922             list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list);
   922             list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list);
   923         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
   923         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);
   924         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
   924         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
   925         return;
   925         return;
   926     }
   926     }
   927 
   927 
   928     // All SDOs are now configured.
   928     // All Sdos are now configured.
   929     ec_fsm_slave_conf_enter_mapping(fsm);
   929     ec_fsm_slave_conf_enter_mapping(fsm);
   930 }
   930 }
   931 
   931 
   932 /*****************************************************************************/
   932 /*****************************************************************************/
   933 
   933 
   934 /**
   934 /**
   935  * Check for PDO mappings to be applied.
   935  * Check for Pdo mappings to be applied.
   936  */
   936  */
   937 
   937 
   938 void ec_fsm_slave_conf_enter_mapping(
   938 void ec_fsm_slave_conf_enter_mapping(
   939         ec_fsm_slave_t *fsm /**< slave state machine */
   939         ec_fsm_slave_t *fsm /**< slave state machine */
   940         )
   940         )
   941 {
   941 {
   942     // start configuring PDO mapping
   942     // start configuring Pdo mapping
   943     fsm->state = ec_fsm_slave_conf_state_mapping;
   943     fsm->state = ec_fsm_slave_conf_state_mapping;
   944     ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave);
   944     ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave);
   945     ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately
   945     ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately
   946 }
   946 }
   947 
   947 
   956         )
   956         )
   957 {
   957 {
   958     if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return;
   958     if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return;
   959 
   959 
   960     if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) {
   960     if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) {
   961         EC_ERR("PDO mapping configuration failed for slave %u.\n",
   961         EC_ERR("Pdo mapping configuration failed for slave %u.\n",
   962                 fsm->slave->ring_position);
   962                 fsm->slave->ring_position);
   963         fsm->slave->error_flag = 1;
   963         fsm->slave->error_flag = 1;
   964         fsm->state = ec_fsm_slave_state_error;
   964         fsm->state = ec_fsm_slave_state_error;
   965         return;
   965         return;
   966     }
   966     }
   995 }
   995 }
   996 
   996 
   997 /*****************************************************************************/
   997 /*****************************************************************************/
   998 
   998 
   999 /**
   999 /**
  1000  * Check for PDO sync managers to be configured.
  1000  * Check for Pdo sync managers to be configured.
  1001  */
  1001  */
  1002 
  1002 
  1003 void ec_fsm_slave_conf_enter_pdo_sync(
  1003 void ec_fsm_slave_conf_enter_pdo_sync(
  1004         ec_fsm_slave_t *fsm /**< slave state machine */
  1004         ec_fsm_slave_t *fsm /**< slave state machine */
  1005         )
  1005         )
  1041 }
  1041 }
  1042 
  1042 
  1043 /*****************************************************************************/
  1043 /*****************************************************************************/
  1044 
  1044 
  1045 /**
  1045 /**
  1046  * Configure PDO sync managers.
  1046  * Configure Pdo sync managers.
  1047  */
  1047  */
  1048 
  1048 
  1049 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *fsm /**< slave state machine */)
  1049 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *fsm /**< slave state machine */)
  1050 {
  1050 {
  1051     ec_datagram_t *datagram = fsm->datagram;
  1051     ec_datagram_t *datagram = fsm->datagram;
  1099     memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);
  1099     memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);
  1100     for (i = 0; i < slave->config->used_fmmus; i++) {
  1100     for (i = 0; i < slave->config->used_fmmus; i++) {
  1101         fmmu = &slave->config->fmmu_configs[i];
  1101         fmmu = &slave->config->fmmu_configs[i];
  1102         if (!(sync = ec_slave_get_pdo_sync(slave, fmmu->dir))) {
  1102         if (!(sync = ec_slave_get_pdo_sync(slave, fmmu->dir))) {
  1103             fsm->state = ec_fsm_slave_state_error;
  1103             fsm->state = ec_fsm_slave_state_error;
  1104             EC_ERR("Failed to determine PDO sync manager for FMMU on slave"
  1104             EC_ERR("Failed to determine Pdo sync manager for FMMU on slave"
  1105                     " %u!\n", slave->ring_position);
  1105                     " %u!\n", slave->ring_position);
  1106             return;
  1106             return;
  1107         }
  1107         }
  1108         ec_fmmu_config_page(fmmu, sync,
  1108         ec_fmmu_config_page(fmmu, sync,
  1109                 datagram->data + EC_FMMU_PAGE_SIZE * i);
  1109                 datagram->data + EC_FMMU_PAGE_SIZE * i);