399 |
399 |
400 EC_INFO("Requesting master %i...\n", master_index); |
400 EC_INFO("Requesting master %i...\n", master_index); |
401 |
401 |
402 if (!(master = ec_find_master(master_index))) goto out_return; |
402 if (!(master = ec_find_master(master_index))) goto out_return; |
403 |
403 |
404 if (master->reserved) { |
404 if (!atomic_dec_and_test(&master->available)) { |
|
405 atomic_inc(&master->available); |
405 EC_ERR("Master %i is already in use!\n", master_index); |
406 EC_ERR("Master %i is already in use!\n", master_index); |
406 goto out_return; |
407 goto out_return; |
407 } |
408 } |
408 master->reserved = 1; |
|
409 |
409 |
410 if (!master->device) { |
410 if (!master->device) { |
411 EC_ERR("Master %i has no assigned device!\n", master_index); |
411 EC_ERR("Master %i has no assigned device!\n", master_index); |
412 goto out_release; |
412 goto out_release; |
413 } |
413 } |
414 |
414 |
415 if (!try_module_get(master->device->module)) { |
415 if (!try_module_get(master->device->module)) { // possible race? |
416 EC_ERR("Failed to reserve device module!\n"); |
416 EC_ERR("Failed to reserve device module!\n"); |
417 goto out_release; |
417 goto out_release; |
418 } |
418 } |
419 |
419 |
420 ec_master_measure_bus_time(master); |
420 if (!master->device->link_state) { |
421 ec_master_idle_stop(master); |
421 EC_ERR("Link is DOWN.\n"); |
422 ec_master_reset(master); |
422 goto out_module_put; |
|
423 } |
|
424 |
|
425 ec_master_reset(master); // also stops idle mode |
423 master->mode = EC_MASTER_MODE_OPERATION; |
426 master->mode = EC_MASTER_MODE_OPERATION; |
424 |
427 |
425 if (!master->device->link_state) EC_WARN("Link is DOWN.\n"); |
428 if (ec_master_measure_bus_time(master)) { |
|
429 EC_ERR("Bus time measuring failed!\n"); |
|
430 goto out_reset; |
|
431 } |
426 |
432 |
427 if (ec_master_bus_scan(master)) { |
433 if (ec_master_bus_scan(master)) { |
428 EC_ERR("Bus scan failed!\n"); |
434 EC_ERR("Bus scan failed!\n"); |
429 goto out_module_put; |
435 goto out_reset; |
430 } |
436 } |
431 |
437 |
432 EC_INFO("Master %i is ready.\n", master_index); |
438 EC_INFO("Successfully requested master %i.\n", master_index); |
433 return master; |
439 return master; |
434 |
440 |
|
441 out_reset: |
|
442 ec_master_reset(master); |
|
443 ec_master_idle_start(master); |
435 out_module_put: |
444 out_module_put: |
436 module_put(master->device->module); |
445 module_put(master->device->module); |
|
446 out_release: |
|
447 atomic_inc(&master->available); |
|
448 out_return: |
|
449 EC_ERR("Failed to request master %i.\n", master_index); |
|
450 return NULL; |
|
451 } |
|
452 |
|
453 /*****************************************************************************/ |
|
454 |
|
455 /** |
|
456 Releases a reserved EtherCAT master. |
|
457 \ingroup RealtimeInterface |
|
458 */ |
|
459 |
|
460 void ecrt_release_master(ec_master_t *master /**< EtherCAT master */) |
|
461 { |
|
462 EC_INFO("Releasing master %i...\n", master->index); |
|
463 |
|
464 if (atomic_read(&master->available)) { |
|
465 EC_ERR("Master %i was never requested!\n", master->index); |
|
466 return; |
|
467 } |
|
468 |
437 ec_master_reset(master); |
469 ec_master_reset(master); |
438 ec_master_idle_start(master); |
470 ec_master_idle_start(master); |
439 out_release: |
|
440 master->reserved = 0; |
|
441 out_return: |
|
442 EC_ERR("Failed requesting master %i.\n", master_index); |
|
443 return NULL; |
|
444 } |
|
445 |
|
446 /*****************************************************************************/ |
|
447 |
|
448 /** |
|
449 Releases a reserved EtherCAT master. |
|
450 \ingroup RealtimeInterface |
|
451 */ |
|
452 |
|
453 void ecrt_release_master(ec_master_t *master /**< EtherCAT master */) |
|
454 { |
|
455 EC_INFO("Releasing master %i...\n", master->index); |
|
456 |
|
457 if (!master->reserved) { |
|
458 EC_ERR("Master %i was never requested!\n", master->index); |
|
459 return; |
|
460 } |
|
461 |
|
462 ec_master_reset(master); |
|
463 ec_master_idle_start(master); |
|
464 |
471 |
465 module_put(master->device->module); |
472 module_put(master->device->module); |
466 master->reserved = 0; |
473 atomic_inc(&master->available); |
467 |
474 |
468 EC_INFO("Released master %i.\n", master->index); |
475 EC_INFO("Successfully released master %i.\n", master->index); |
469 return; |
476 return; |
470 } |
477 } |
471 |
478 |
472 /*****************************************************************************/ |
479 /*****************************************************************************/ |
473 |
480 |