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