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