374 ec_fsm_master_t *fsm /**< master state machine */ |
375 ec_fsm_master_t *fsm /**< master state machine */ |
375 ) |
376 ) |
376 { |
377 { |
377 ec_master_t *master = fsm->master; |
378 ec_master_t *master = fsm->master; |
378 ec_master_sdo_request_t *request; |
379 ec_master_sdo_request_t *request; |
|
380 ec_sdo_request_t *req; |
379 ec_slave_t *slave; |
381 ec_slave_t *slave; |
380 |
382 |
381 // search the first request to be processed |
383 // search for internal requests to be processed |
|
384 list_for_each_entry(slave, &master->slaves, list) { |
|
385 if (!slave->config) |
|
386 continue; |
|
387 list_for_each_entry(req, &slave->config->sdo_requests, list) { |
|
388 if (req->state == EC_REQUEST_QUEUED) { |
|
389 req->state = EC_REQUEST_IN_PROGRESS; |
|
390 |
|
391 if (slave->current_state == EC_SLAVE_STATE_INIT || |
|
392 slave->error_flag) { |
|
393 req->state = EC_REQUEST_FAILURE; |
|
394 continue; |
|
395 } |
|
396 |
|
397 if (master->debug_level) |
|
398 EC_DBG("Processing Sdo request for slave %u...\n", |
|
399 slave->ring_position); |
|
400 |
|
401 fsm->idle = 0; |
|
402 fsm->sdo_request = req; |
|
403 fsm->slave = slave; |
|
404 fsm->state = ec_fsm_master_state_sdo_request; |
|
405 ec_fsm_coe_upload(&fsm->fsm_coe, slave, req); |
|
406 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
407 return 1; |
|
408 } |
|
409 } |
|
410 } |
|
411 |
|
412 // search the first external request to be processed |
382 while (1) { |
413 while (1) { |
383 down(&master->sdo_sem); |
414 down(&master->sdo_sem); |
384 if (list_empty(&master->slave_sdo_requests)) { |
415 if (list_empty(&master->slave_sdo_requests)) { |
385 up(&master->sdo_sem); |
416 up(&master->sdo_sem); |
386 break; |
417 break; |
408 EC_DBG("Processing Sdo request for slave %i...\n", |
439 EC_DBG("Processing Sdo request for slave %i...\n", |
409 slave->ring_position); |
440 slave->ring_position); |
410 |
441 |
411 // Start uploading Sdo |
442 // Start uploading Sdo |
412 fsm->idle = 0; |
443 fsm->idle = 0; |
413 fsm->sdo_request = request; |
444 fsm->sdo_request = &request->req; |
|
445 fsm->slave = slave; |
414 fsm->state = ec_fsm_master_state_sdo_request; |
446 fsm->state = ec_fsm_master_state_sdo_request; |
415 ec_fsm_coe_upload(&fsm->fsm_coe, slave, &request->req); |
447 ec_fsm_coe_upload(&fsm->fsm_coe, slave, &request->req); |
416 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
448 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
417 return 1; |
449 return 1; |
418 } |
450 } |
497 // check for pending slave configurations |
529 // check for pending slave configurations |
498 if (ec_fsm_master_action_configure(fsm)) |
530 if (ec_fsm_master_action_configure(fsm)) |
499 return; |
531 return; |
500 } |
532 } |
501 |
533 |
502 // Check for a pending Sdo request |
534 // Check for pending Sdo requests |
503 if (ec_fsm_master_action_process_sdo(fsm)) |
535 if (ec_fsm_master_action_process_sdo(fsm)) |
504 return; |
536 return; |
505 |
537 |
506 if (master->mode == EC_MASTER_MODE_IDLE) { |
538 if (master->mode == EC_MASTER_MODE_IDLE) { |
507 |
539 |
1024 */ |
1056 */ |
1025 |
1057 |
1026 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *fsm /**< master state machine */) |
1058 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *fsm /**< master state machine */) |
1027 { |
1059 { |
1028 ec_master_t *master = fsm->master; |
1060 ec_master_t *master = fsm->master; |
1029 ec_master_sdo_request_t *request = fsm->sdo_request; |
1061 ec_sdo_request_t *request = fsm->sdo_request; |
1030 |
1062 |
1031 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
1063 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
1032 |
1064 |
1033 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1065 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1034 EC_DBG("Failed to process Sdo request for slave %i.\n", |
1066 EC_DBG("Failed to process Sdo request for slave %u.\n", |
1035 fsm->slave->ring_position); |
1067 fsm->slave->ring_position); |
1036 request->req.state = EC_REQUEST_FAILURE; |
1068 request->state = EC_REQUEST_FAILURE; |
1037 wake_up(&master->sdo_queue); |
1069 wake_up(&master->sdo_queue); |
1038 fsm->state = ec_fsm_master_state_error; |
1070 fsm->state = ec_fsm_master_state_error; |
1039 return; |
1071 return; |
1040 } |
1072 } |
1041 |
1073 |
1042 // Sdo request finished |
1074 // Sdo request finished |
1043 request->req.state = EC_REQUEST_COMPLETE; |
1075 request->state = EC_REQUEST_COMPLETE; |
1044 wake_up(&master->sdo_queue); |
1076 wake_up(&master->sdo_queue); |
1045 |
1077 |
1046 if (master->debug_level) |
1078 if (master->debug_level) |
1047 EC_DBG("Finished Sdo request for slave %i.\n", |
1079 EC_DBG("Finished Sdo request for slave %u.\n", |
1048 fsm->slave->ring_position); |
1080 fsm->slave->ring_position); |
1049 |
1081 |
1050 // check for another Sdo request |
1082 // check for another Sdo request |
1051 if (ec_fsm_master_action_process_sdo(fsm)) |
1083 if (ec_fsm_master_action_process_sdo(fsm)) |
1052 return; // processing another request |
1084 return; // processing another request |