315 return 0; |
315 return 0; |
316 } |
316 } |
317 |
317 |
318 /*****************************************************************************/ |
318 /*****************************************************************************/ |
319 |
319 |
320 /** Check for pending Sdo requests and process one. |
320 /** Check for pending SDO requests and process one. |
321 * |
321 * |
322 * \return non-zero, if an Sdo request is processed. |
322 * \return non-zero, if an SDO request is processed. |
323 */ |
323 */ |
324 int ec_fsm_master_action_process_sdo( |
324 int ec_fsm_master_action_process_sdo( |
325 ec_fsm_master_t *fsm /**< Master state machine. */ |
325 ec_fsm_master_t *fsm /**< Master state machine. */ |
326 ) |
326 ) |
327 { |
327 { |
340 if (req->state == EC_REQUEST_QUEUED) { |
340 if (req->state == EC_REQUEST_QUEUED) { |
341 |
341 |
342 if (ec_sdo_request_timed_out(req)) { |
342 if (ec_sdo_request_timed_out(req)) { |
343 req->state = EC_REQUEST_FAILURE; |
343 req->state = EC_REQUEST_FAILURE; |
344 if (master->debug_level) |
344 if (master->debug_level) |
345 EC_DBG("Sdo request for slave %u timed out...\n", |
345 EC_DBG("SDO request for slave %u timed out...\n", |
346 slave->ring_position); |
346 slave->ring_position); |
347 continue; |
347 continue; |
348 } |
348 } |
349 |
349 |
350 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
350 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
352 continue; |
352 continue; |
353 } |
353 } |
354 |
354 |
355 req->state = EC_REQUEST_BUSY; |
355 req->state = EC_REQUEST_BUSY; |
356 if (master->debug_level) |
356 if (master->debug_level) |
357 EC_DBG("Processing Sdo request for slave %u...\n", |
357 EC_DBG("Processing SDO request for slave %u...\n", |
358 slave->ring_position); |
358 slave->ring_position); |
359 |
359 |
360 fsm->idle = 0; |
360 fsm->idle = 0; |
361 fsm->sdo_request = req; |
361 fsm->sdo_request = req; |
362 fsm->slave = slave; |
362 fsm->slave = slave; |
379 list_del_init(&request->list); // dequeue |
379 list_del_init(&request->list); // dequeue |
380 request->req.state = EC_REQUEST_BUSY; |
380 request->req.state = EC_REQUEST_BUSY; |
381 |
381 |
382 slave = request->slave; |
382 slave = request->slave; |
383 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
383 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
384 EC_ERR("Discarding Sdo request, slave %u is in INIT.\n", |
384 EC_ERR("Discarding SDO request, slave %u is in INIT.\n", |
385 slave->ring_position); |
385 slave->ring_position); |
386 request->req.state = EC_REQUEST_FAILURE; |
386 request->req.state = EC_REQUEST_FAILURE; |
387 wake_up(&master->sdo_queue); |
387 wake_up(&master->sdo_queue); |
388 continue; |
388 continue; |
389 } |
389 } |
390 |
390 |
391 // Found pending Sdo request. Execute it! |
391 // Found pending SDO request. Execute it! |
392 if (master->debug_level) |
392 if (master->debug_level) |
393 EC_DBG("Processing Sdo request for slave %u...\n", |
393 EC_DBG("Processing SDO request for slave %u...\n", |
394 slave->ring_position); |
394 slave->ring_position); |
395 |
395 |
396 // Start uploading Sdo |
396 // Start uploading SDO |
397 fsm->idle = 0; |
397 fsm->idle = 0; |
398 fsm->sdo_request = &request->req; |
398 fsm->sdo_request = &request->req; |
399 fsm->slave = slave; |
399 fsm->slave = slave; |
400 fsm->state = ec_fsm_master_state_sdo_request; |
400 fsm->state = ec_fsm_master_state_sdo_request; |
401 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
401 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
417 ) |
417 ) |
418 { |
418 { |
419 ec_master_t *master = fsm->master; |
419 ec_master_t *master = fsm->master; |
420 ec_slave_t *slave; |
420 ec_slave_t *slave; |
421 |
421 |
422 // Check for pending Sdo requests |
422 // Check for pending SDO requests |
423 if (ec_fsm_master_action_process_sdo(fsm)) |
423 if (ec_fsm_master_action_process_sdo(fsm)) |
424 return; |
424 return; |
425 |
425 |
426 // check, if slaves have an Sdo dictionary to read out. |
426 // check, if slaves have an SDO dictionary to read out. |
427 for (slave = master->slaves; |
427 for (slave = master->slaves; |
428 slave < master->slaves + master->slave_count; |
428 slave < master->slaves + master->slave_count; |
429 slave++) { |
429 slave++) { |
430 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE) |
430 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE) |
431 || (slave->sii.has_general |
431 || (slave->sii.has_general |
435 || slave->current_state == EC_SLAVE_STATE_UNKNOWN |
435 || slave->current_state == EC_SLAVE_STATE_UNKNOWN |
436 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ |
436 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ |
437 ) continue; |
437 ) continue; |
438 |
438 |
439 if (master->debug_level) { |
439 if (master->debug_level) { |
440 EC_DBG("Fetching Sdo dictionary from slave %u.\n", |
440 EC_DBG("Fetching SDO dictionary from slave %u.\n", |
441 slave->ring_position); |
441 slave->ring_position); |
442 } |
442 } |
443 |
443 |
444 slave->sdo_dictionary_fetched = 1; |
444 slave->sdo_dictionary_fetched = 1; |
445 |
445 |
446 // start fetching Sdo dictionary |
446 // start fetching SDO dictionary |
447 fsm->idle = 0; |
447 fsm->idle = 0; |
448 fsm->slave = slave; |
448 fsm->slave = slave; |
449 fsm->state = ec_fsm_master_state_sdo_dictionary; |
449 fsm->state = ec_fsm_master_state_sdo_dictionary; |
450 ec_fsm_coe_dictionary(&fsm->fsm_coe, slave); |
450 ec_fsm_coe_dictionary(&fsm->fsm_coe, slave); |
451 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
451 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
805 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
805 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
806 ec_fsm_master_restart(fsm); |
806 ec_fsm_master_restart(fsm); |
807 return; |
807 return; |
808 } |
808 } |
809 |
809 |
810 // Sdo dictionary fetching finished |
810 // SDO dictionary fetching finished |
811 |
811 |
812 if (master->debug_level) { |
812 if (master->debug_level) { |
813 unsigned int sdo_count, entry_count; |
813 unsigned int sdo_count, entry_count; |
814 ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count); |
814 ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count); |
815 EC_DBG("Fetched %u Sdos and %u entries from slave %u.\n", |
815 EC_DBG("Fetched %u SDOs and %u entries from slave %u.\n", |
816 sdo_count, entry_count, slave->ring_position); |
816 sdo_count, entry_count, slave->ring_position); |
817 } |
817 } |
818 |
818 |
819 // attach pdo names from dictionary |
819 // attach pdo names from dictionary |
820 ec_slave_attach_pdo_names(slave); |
820 ec_slave_attach_pdo_names(slave); |
834 ec_sdo_request_t *request = fsm->sdo_request; |
834 ec_sdo_request_t *request = fsm->sdo_request; |
835 |
835 |
836 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
836 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
837 |
837 |
838 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
838 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
839 EC_DBG("Failed to process Sdo request for slave %u.\n", |
839 EC_DBG("Failed to process SDO request for slave %u.\n", |
840 fsm->slave->ring_position); |
840 fsm->slave->ring_position); |
841 request->state = EC_REQUEST_FAILURE; |
841 request->state = EC_REQUEST_FAILURE; |
842 wake_up(&master->sdo_queue); |
842 wake_up(&master->sdo_queue); |
843 ec_fsm_master_restart(fsm); |
843 ec_fsm_master_restart(fsm); |
844 return; |
844 return; |
845 } |
845 } |
846 |
846 |
847 // Sdo request finished |
847 // SDO request finished |
848 request->state = EC_REQUEST_SUCCESS; |
848 request->state = EC_REQUEST_SUCCESS; |
849 wake_up(&master->sdo_queue); |
849 wake_up(&master->sdo_queue); |
850 |
850 |
851 if (master->debug_level) |
851 if (master->debug_level) |
852 EC_DBG("Finished Sdo request for slave %u.\n", |
852 EC_DBG("Finished SDO request for slave %u.\n", |
853 fsm->slave->ring_position); |
853 fsm->slave->ring_position); |
854 |
854 |
855 // check for another Sdo request |
855 // check for another SDO request |
856 if (ec_fsm_master_action_process_sdo(fsm)) |
856 if (ec_fsm_master_action_process_sdo(fsm)) |
857 return; // processing another request |
857 return; // processing another request |
858 |
858 |
859 ec_fsm_master_restart(fsm); |
859 ec_fsm_master_restart(fsm); |
860 } |
860 } |