master/fsm_slave_config.c
changeset 1327 4d179b06dd3c
parent 1326 ef907b0b5125
child 1337 0253c74d0940
equal deleted inserted replaced
1326:ef907b0b5125 1327:4d179b06dd3c
    72 void ec_fsm_slave_config_init(
    72 void ec_fsm_slave_config_init(
    73         ec_fsm_slave_config_t *fsm, /**< slave state machine */
    73         ec_fsm_slave_config_t *fsm, /**< slave state machine */
    74         ec_datagram_t *datagram, /**< datagram structure to use */
    74         ec_datagram_t *datagram, /**< datagram structure to use */
    75         ec_fsm_change_t *fsm_change, /**< State change state machine to use. */
    75         ec_fsm_change_t *fsm_change, /**< State change state machine to use. */
    76         ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */
    76         ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */
    77         ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration state machine to use. */
    77         ec_fsm_pdo_t *fsm_pdo /**< PDO configuration state machine to use. */
    78         )
    78         )
    79 {
    79 {
    80     ec_sdo_request_init(&fsm->request_copy);
    80     ec_sdo_request_init(&fsm->request_copy);
    81 
    81 
    82     fsm->datagram = datagram;
    82     fsm->datagram = datagram;
   434     ec_fsm_slave_config_enter_sdo_conf(fsm);
   434     ec_fsm_slave_config_enter_sdo_conf(fsm);
   435 }
   435 }
   436 
   436 
   437 /*****************************************************************************/
   437 /*****************************************************************************/
   438 
   438 
   439 /** Check for Sdo configurations to be applied.
   439 /** Check for SDO configurations to be applied.
   440  */
   440  */
   441 void ec_fsm_slave_config_enter_sdo_conf(
   441 void ec_fsm_slave_config_enter_sdo_conf(
   442         ec_fsm_slave_config_t *fsm /**< slave state machine */
   442         ec_fsm_slave_config_t *fsm /**< slave state machine */
   443         )
   443         )
   444 {
   444 {
   445     ec_slave_t *slave = fsm->slave;
   445     ec_slave_t *slave = fsm->slave;
   446 
   446 
   447     // No CoE configuration to be applied?
   447     // No CoE configuration to be applied?
   448     if (list_empty(&slave->config->sdo_configs)) { // skip Sdo configuration
   448     if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration
   449         ec_fsm_slave_config_enter_pdo_conf(fsm);
   449         ec_fsm_slave_config_enter_pdo_conf(fsm);
   450         return;
   450         return;
   451     }
   451     }
   452 
   452 
   453     // start Sdo configuration
   453     // start SDO configuration
   454     fsm->state = ec_fsm_slave_config_state_sdo_conf;
   454     fsm->state = ec_fsm_slave_config_state_sdo_conf;
   455     fsm->request = list_entry(fsm->slave->config->sdo_configs.next,
   455     fsm->request = list_entry(fsm->slave->config->sdo_configs.next,
   456             ec_sdo_request_t, list);
   456             ec_sdo_request_t, list);
   457     ec_sdo_request_copy(&fsm->request_copy, fsm->request);
   457     ec_sdo_request_copy(&fsm->request_copy, fsm->request);
   458     ecrt_sdo_request_write(&fsm->request_copy);
   458     ecrt_sdo_request_write(&fsm->request_copy);
   469         )
   469         )
   470 {
   470 {
   471     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   471     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   472 
   472 
   473     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   473     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   474         EC_ERR("Sdo configuration failed for slave %u.\n",
   474         EC_ERR("SDO configuration failed for slave %u.\n",
   475                 fsm->slave->ring_position);
   475                 fsm->slave->ring_position);
   476         fsm->slave->error_flag = 1;
   476         fsm->slave->error_flag = 1;
   477         fsm->state = ec_fsm_slave_config_state_error;
   477         fsm->state = ec_fsm_slave_config_state_error;
   478         return;
   478         return;
   479     }
   479     }
   481     if (!fsm->slave->config) { // config removed in the meantime
   481     if (!fsm->slave->config) { // config removed in the meantime
   482         ec_fsm_slave_config_reconfigure(fsm);
   482         ec_fsm_slave_config_reconfigure(fsm);
   483         return;
   483         return;
   484     }
   484     }
   485 
   485 
   486     // Another Sdo to configure?
   486     // Another SDO to configure?
   487     if (fsm->request->list.next != &fsm->slave->config->sdo_configs) {
   487     if (fsm->request->list.next != &fsm->slave->config->sdo_configs) {
   488         fsm->request = list_entry(fsm->request->list.next,
   488         fsm->request = list_entry(fsm->request->list.next,
   489                 ec_sdo_request_t, list);
   489                 ec_sdo_request_t, list);
   490         ec_sdo_request_copy(&fsm->request_copy, fsm->request);
   490         ec_sdo_request_copy(&fsm->request_copy, fsm->request);
   491         ecrt_sdo_request_write(&fsm->request_copy);
   491         ecrt_sdo_request_write(&fsm->request_copy);
   492         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
   492         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
   493         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   493         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   494         return;
   494         return;
   495     }
   495     }
   496 
   496 
   497     // All Sdos are now configured.
   497     // All SDOs are now configured.
   498     ec_fsm_slave_config_enter_pdo_conf(fsm);
   498     ec_fsm_slave_config_enter_pdo_conf(fsm);
   499 }
   499 }
   500 
   500 
   501 /*****************************************************************************/
   501 /*****************************************************************************/
   502 
   502 
   504  */
   504  */
   505 void ec_fsm_slave_config_enter_pdo_conf(
   505 void ec_fsm_slave_config_enter_pdo_conf(
   506         ec_fsm_slave_config_t *fsm /**< slave state machine */
   506         ec_fsm_slave_config_t *fsm /**< slave state machine */
   507         )
   507         )
   508 {
   508 {
   509     // Start configuring Pdos
   509     // Start configuring PDOs
   510     ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave);
   510     ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave);
   511     fsm->state = ec_fsm_slave_config_state_pdo_conf;
   511     fsm->state = ec_fsm_slave_config_state_pdo_conf;
   512     fsm->state(fsm); // execute immediately
   512     fsm->state(fsm); // execute immediately
   513 }
   513 }
   514 
   514 
   527         ec_fsm_slave_config_reconfigure(fsm);
   527         ec_fsm_slave_config_reconfigure(fsm);
   528         return;
   528         return;
   529     }
   529     }
   530 
   530 
   531     if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
   531     if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
   532         EC_WARN("Pdo configuration failed on slave %u.\n",
   532         EC_WARN("PDO configuration failed on slave %u.\n",
   533                 fsm->slave->ring_position);
   533                 fsm->slave->ring_position);
   534     }
   534     }
   535 
   535 
   536     ec_fsm_slave_config_enter_pdo_sync(fsm);
   536     ec_fsm_slave_config_enter_pdo_sync(fsm);
   537 }
   537 }
   538 
   538 
   539 /*****************************************************************************/
   539 /*****************************************************************************/
   540 
   540 
   541 /** Check for Pdo sync managers to be configured.
   541 /** Check for PDO sync managers to be configured.
   542  */
   542  */
   543 void ec_fsm_slave_config_enter_pdo_sync(
   543 void ec_fsm_slave_config_enter_pdo_sync(
   544         ec_fsm_slave_config_t *fsm /**< slave state machine */
   544         ec_fsm_slave_config_t *fsm /**< slave state machine */
   545         )
   545         )
   546 {
   546 {
   557     } else {
   557     } else {
   558         offset = 0;
   558         offset = 0;
   559     }
   559     }
   560 
   560 
   561     if (slave->sii.sync_count <= offset) {
   561     if (slave->sii.sync_count <= offset) {
   562         // no Pdo sync managers to configure
   562         // no PDO sync managers to configure
   563         ec_fsm_slave_config_enter_fmmu(fsm);
   563         ec_fsm_slave_config_enter_fmmu(fsm);
   564         return;
   564         return;
   565     }
   565     }
   566 
   566 
   567     num_pdo_syncs = slave->sii.sync_count - offset;
   567     num_pdo_syncs = slave->sii.sync_count - offset;
   585     fsm->state = ec_fsm_slave_config_state_pdo_sync;
   585     fsm->state = ec_fsm_slave_config_state_pdo_sync;
   586 }
   586 }
   587 
   587 
   588 /*****************************************************************************/
   588 /*****************************************************************************/
   589 
   589 
   590 /** Configure Pdo sync managers.
   590 /** Configure PDO sync managers.
   591  */
   591  */
   592 void ec_fsm_slave_config_state_pdo_sync(
   592 void ec_fsm_slave_config_state_pdo_sync(
   593         ec_fsm_slave_config_t *fsm /**< slave state machine */
   593         ec_fsm_slave_config_t *fsm /**< slave state machine */
   594         )
   594         )
   595 {
   595 {
   659     for (i = 0; i < slave->config->used_fmmus; i++) {
   659     for (i = 0; i < slave->config->used_fmmus; i++) {
   660         fmmu = &slave->config->fmmu_configs[i];
   660         fmmu = &slave->config->fmmu_configs[i];
   661         if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) {
   661         if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) {
   662             slave->error_flag = 1;
   662             slave->error_flag = 1;
   663             fsm->state = ec_fsm_slave_config_state_error;
   663             fsm->state = ec_fsm_slave_config_state_error;
   664             EC_ERR("Failed to determine Pdo sync manager for FMMU on slave"
   664             EC_ERR("Failed to determine PDO sync manager for FMMU on slave"
   665                     " %u!\n", slave->ring_position);
   665                     " %u!\n", slave->ring_position);
   666             return;
   666             return;
   667         }
   667         }
   668         ec_fmmu_config_page(fmmu, sync,
   668         ec_fmmu_config_page(fmmu, sync,
   669                 datagram->data + EC_FMMU_PAGE_SIZE * i);
   669                 datagram->data + EC_FMMU_PAGE_SIZE * i);