521 ) |
521 ) |
522 { |
522 { |
523 ec_master_t *master = fsm->master; |
523 ec_master_t *master = fsm->master; |
524 ec_slave_t *slave; |
524 ec_slave_t *slave; |
525 char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE]; |
525 char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE]; |
526 ec_sdo_request_t *request, *next_request; |
|
527 |
526 |
528 // check if any slaves are not in the state, they're supposed to be |
527 // check if any slaves are not in the state, they're supposed to be |
529 list_for_each_entry(slave, &master->slaves, list) { |
528 list_for_each_entry(slave, &master->slaves, list) { |
530 if (slave->error_flag |
529 if (slave->error_flag |
531 || !slave->online |
530 || !slave->online |
556 } |
555 } |
557 |
556 |
558 // Check, if EoE processing has to be started |
557 // Check, if EoE processing has to be started |
559 ec_master_eoe_start(master); |
558 ec_master_eoe_start(master); |
560 |
559 |
561 // check, if there are pending SDO requests |
560 // Check for a pending SDO request |
562 list_for_each_entry_safe(request, next_request, |
561 if (master->sdo_seq_master != master->sdo_seq_user) { |
563 &master->sdo_requests, queue) { |
562 if (master->debug_level) |
564 // TODO: critical section! |
563 EC_DBG("Processing SDO request...\n"); |
565 list_del_init(&request->queue); |
564 slave = master->sdo_request->sdo->slave; |
566 |
|
567 slave = request->sdo->slave; |
|
568 |
|
569 if (slave->current_state == EC_SLAVE_STATE_INIT |
565 if (slave->current_state == EC_SLAVE_STATE_INIT |
570 || !slave->online |
566 || !slave->online |
571 || slave->error_flag) { |
567 || slave->error_flag) { |
572 request->return_code = -1; |
568 EC_ERR("Failed to process SDO request, slave %i not ready.\n", |
573 wake_up_interruptible(&master->sdo_wait_queue); |
569 slave->ring_position); |
574 continue; |
570 master->sdo_request->return_code = -1; |
575 } |
571 master->sdo_seq_master++; |
576 |
572 } |
577 // start uploading SDO |
573 else { |
578 fsm->slave = slave; |
574 // start uploading SDO |
579 fsm->master_state = ec_fsm_master_sdo_request; |
575 fsm->slave = slave; |
580 fsm->sdo_request = request; |
576 fsm->master_state = ec_fsm_master_sdo_request; |
581 ec_fsm_coe_upload(&fsm->fsm_coe, slave, request); |
577 fsm->sdo_request = master->sdo_request; |
582 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
578 ec_fsm_coe_upload(&fsm->fsm_coe, slave, fsm->sdo_request); |
583 return; |
579 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
580 return; |
|
581 } |
584 } |
582 } |
585 |
583 |
586 // check, if slaves have an SDO dictionary to read out. |
584 // check, if slaves have an SDO dictionary to read out. |
587 list_for_each_entry(slave, &master->slaves, list) { |
585 list_for_each_entry(slave, &master->slaves, list) { |
588 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE) |
586 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE) |
1032 |
1030 |
1033 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
1031 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
1034 |
1032 |
1035 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1033 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1036 request->return_code = -1; |
1034 request->return_code = -1; |
1037 wake_up_interruptible(&master->sdo_wait_queue); |
1035 master->sdo_seq_master++; |
1038 fsm->master_state = ec_fsm_master_start; |
1036 fsm->master_state = ec_fsm_master_start; |
1039 fsm->master_state(fsm); // execute immediately |
1037 fsm->master_state(fsm); // execute immediately |
1040 return; |
1038 return; |
1041 } |
1039 } |
1042 |
1040 |
1043 // SDO dictionary fetching finished |
1041 // SDO dictionary fetching finished |
1044 |
1042 |
1045 request->return_code = 1; |
1043 request->return_code = 1; |
1046 wake_up_interruptible(&master->sdo_wait_queue); |
1044 master->sdo_seq_master++; |
1047 |
1045 |
1048 // restart master state machine. |
1046 // restart master state machine. |
1049 fsm->master_state = ec_fsm_master_start; |
1047 fsm->master_state = ec_fsm_master_start; |
1050 fsm->master_state(fsm); // execute immediately |
1048 fsm->master_state(fsm); // execute immediately |
1051 } |
1049 } |