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 { |
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); |