Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/master.c
changeset 2164 cf4db3c282d2
parent 2150 5144a4bc6184
child 2165 1d94f77784a0
equal deleted inserted replaced
2163:d6d49dcaf7a5 2164:cf4db3c282d2 
  2400 
  2400 
  2401 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
 
  2401 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
 
  2402         uint16_t index, uint8_t subindex, uint8_t *data,
 
  2402         uint16_t index, uint8_t subindex, uint8_t *data,
 
  2403         size_t data_size, uint32_t *abort_code)
 
  2403         size_t data_size, uint32_t *abort_code)
 
  2404 {
 
  2404 {
 
  2405     ec_master_sdo_request_t* request;
 
  2405     ec_master_sdo_request_t *request;
 
  2406     int retval;
 
  2406     int retval;
 
  2407 
  2407 
  2408     EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
 
  2408     EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
 
  2409             " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
 
  2409             " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
 
  2410             " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
 
  2410             " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
 
  2484 
  2484 
  2485 int ecrt_master_sdo_download_complete(ec_master_t *master,
 
  2485 int ecrt_master_sdo_download_complete(ec_master_t *master,
 
  2486         uint16_t slave_position, uint16_t index, uint8_t *data,
 
  2486         uint16_t slave_position, uint16_t index, uint8_t *data,
 
  2487         size_t data_size, uint32_t *abort_code)
 
  2487         size_t data_size, uint32_t *abort_code)
 
  2488 {
 
  2488 {
 
  2489     ec_master_sdo_request_t request;
 
  2489     ec_master_sdo_request_t *request;
 
       
  2490     int retval;
 
  2490 
  2491 
  2491     EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
 
  2492     EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
 
  2492             " slave_position = %u, index = 0x%04X,"
 
  2493             " slave_position = %u, index = 0x%04X,"
 
  2493             " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
 
  2494             " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
 
  2494             __func__, master, slave_position, index, data, data_size,
 
  2495             __func__, master, slave_position, index, data, data_size,
 
  2497     if (!data_size) {
 
  2498     if (!data_size) {
 
  2498         EC_MASTER_ERR(master, "Zero data size!\n");
 
  2499         EC_MASTER_ERR(master, "Zero data size!\n");
 
  2499         return -EINVAL;
 
  2500         return -EINVAL;
 
  2500     }
 
  2501     }
 
  2501 
  2502 
  2502     ec_sdo_request_init(&request.req);
 
  2503     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2503     ec_sdo_request_address(&request.req, index, 0);
 
  2504     if (!request) {
 
  2504     if (ec_sdo_request_alloc(&request.req, data_size)) {
 
       
  2505         ec_sdo_request_clear(&request.req);
 
       
  2506         return -ENOMEM;
 
  2505         return -ENOMEM;
 
  2507     }
 
  2506     }
 
  2508 
  2507     kref_init(&request->refcount);
 
  2509     request.req.complete_access = 1;
 
  2508 
  2510     memcpy(request.req.data, data, data_size);
 
  2509     ec_sdo_request_init(&request->req);
 
  2511     request.req.data_size = data_size;
 
  2510     ec_sdo_request_address(&request->req, index, 0);
 
  2512     ecrt_sdo_request_write(&request.req);
 
  2511     if (ec_sdo_request_alloc(&request->req, data_size)) {
 
  2513 
  2512         kref_put(&request->refcount, ec_master_sdo_request_release);
 
  2514     if (down_interruptible(&master->master_sem))
 
  2513         return -ENOMEM;
 
       
  2514     }
 
       
  2515 
       
  2516     request->req.complete_access = 1;
 
       
  2517     memcpy(request->req.data, data, data_size);
 
       
  2518     request->req.data_size = data_size;
 
       
  2519     ecrt_sdo_request_write(&request->req);
 
       
  2520 
       
  2521     if (ec_mutex_lock_interruptible(&master->master_mutex)) {
 
       
  2522         kref_put(&request->refcount, ec_master_sdo_request_release);
 
  2515         return -EINTR;
 
  2523         return -EINTR;
 
  2516 
  2524     }
 
  2517     if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) {
 
  2525 
  2518         up(&master->master_sem);
 
  2526     if (!(request->slave = ec_master_find_slave(
 
       
  2527                     master, 0, slave_position))) {
 
       
  2528         ec_mutex_unlock(&master->master_mutex);
 
  2519         EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
 
  2529         EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
 
  2520         ec_sdo_request_clear(&request.req);
 
  2530         kref_put(&request->refcount, ec_master_sdo_request_release);
 
  2521         return -EINVAL;
 
  2531         return -EINVAL;
 
  2522     }
 
  2532     }
 
  2523 
  2533 
  2524     EC_SLAVE_DBG(request.slave, 1, "Schedule SDO download request"
 
  2534     EC_SLAVE_DBG(request->slave, 1, "Schedule SDO download request %p"
 
  2525             " (complete access).\n");
 
  2535             " (complete access).\n", request);
 
  2526 
  2536 
  2527     // schedule request.
 
  2537     // schedule request
 
  2528     list_add_tail(&request.list, &request.slave->slave_sdo_requests);
 
  2538     kref_get(&request->refcount);
 
  2529 
  2539     list_add_tail(&request->list, &request->slave->slave_sdo_requests);
 
  2530     up(&master->master_sem);
 
  2540 
       
  2541     ec_mutex_unlock(&master->master_mutex);
 
  2531 
  2542 
  2532     // wait for processing through FSM
 
  2543     // wait for processing through FSM
 
  2533     if (wait_event_interruptible(request.slave->sdo_queue,
 
  2544     if (wait_event_interruptible(request->slave->sdo_queue,
 
  2534                 request.req.state != EC_INT_REQUEST_QUEUED)) {
 
  2545        ((request->req.state == EC_INT_REQUEST_SUCCESS) ||
 
       
  2546         (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2535         // interrupted by signal
 
  2547         // interrupted by signal
 
  2536         down(&master->master_sem);
 
  2548         kref_put(&request->refcount, ec_master_sdo_request_release);
 
  2537         if (request.req.state == EC_INT_REQUEST_QUEUED) {
 
  2549         return -EINTR;
 
  2538             list_del(&request.list);
 
  2550     }
 
  2539             up(&master->master_sem);
 
  2551 
  2540             ec_sdo_request_clear(&request.req);
 
  2552     EC_SLAVE_DBG(request->slave, 1, "Finished SDO download request %p"
 
  2541             return -EINTR;
 
  2553             " (complete access).\n", request);
 
  2542         }
 
  2554 
  2543         // request already processing: interrupt not possible.
 
  2555     *abort_code = request->req.abort_code;
 
  2544         up(&master->master_sem);
 
  2556 
  2545     }
 
  2557     if (request->req.state == EC_INT_REQUEST_SUCCESS) {
 
  2546 
  2558         retval = 0;
 
  2547     // wait until master FSM has finished processing
 
  2559     } else if (request->req.errno) {
 
  2548     wait_event(request.slave->sdo_queue,
 
  2560         retval = -request->req.errno;
 
  2549             request.req.state != EC_INT_REQUEST_BUSY);
 
       
  2550 
       
  2551     EC_SLAVE_DBG(request.slave, 1, "Finished SDO download request"
 
       
  2552             " (complete access).\n");
 
       
  2553 
       
  2554     *abort_code = request.req.abort_code;
 
       
  2555 
       
  2556     if (request.req.state == EC_INT_REQUEST_SUCCESS) {
 
       
  2557         return 0;
 
       
  2558     } else if (request.req.errno) {
 
       
  2559         return -request.req.errno;
 
       
  2560     } else {
 
  2561     } else {
 
  2561         return -EIO;
 
  2562         retval = -EIO;
 
  2562     }
 
  2563     }
 
       
  2564 
       
  2565     kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2566     return retval;
 
  2563 }
 
  2567 }
 
  2564 
  2568 
  2565 /*****************************************************************************/
 
  2569 /*****************************************************************************/
 
  2566 
  2570 
  2567 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
 
  2571 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
 
  2568         uint16_t index, uint8_t subindex, uint8_t *target,
 
  2572         uint16_t index, uint8_t subindex, uint8_t *target,
 
  2569         size_t target_size, size_t *result_size, uint32_t *abort_code)
 
  2573         size_t target_size, size_t *result_size, uint32_t *abort_code)
 
  2570 {
 
  2574 {
 
  2571     ec_master_sdo_request_t* request;
 
  2575     ec_master_sdo_request_t *request;
 
  2572     int retval;
 
  2576     int retval;
 
  2573 
  2577 
  2574     EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
 
  2578     EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
 
  2575             " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
 
  2579             " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
 
  2576             " target = 0x%p, target_size = %zu, result_size = 0x%p, "
 
  2580             " target = 0x%p, target_size = %zu, result_size = 0x%p, "
 
  2577             " abort_code = 0x%p)\n",
 
  2581             " abort_code = 0x%p)\n",
 
  2578             __func__, master, slave_position, index, subindex, target,
 
  2582             __func__, master, slave_position, index, subindex, target,
 
  2579             target_size, result_size, abort_code);
 
  2583             target_size, result_size, abort_code);
 
  2580 
  2584 
  2581     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2585     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2582     if (!request)
 
  2586     if (!request) {
 
  2583         return -ENOMEM;
 
  2587         return -ENOMEM;
 
       
  2588     }
 
  2584     kref_init(&request->refcount);
 
  2589     kref_init(&request->refcount);
 
  2585 
  2590 
  2586     ec_sdo_request_init(&request->req);
 
  2591     ec_sdo_request_init(&request->req);
 
  2587     ec_sdo_request_address(&request->req, index, subindex);
 
  2592     ec_sdo_request_address(&request->req, index, subindex);
 
  2588     ecrt_sdo_request_read(&request->req);
 
  2593     ecrt_sdo_request_read(&request->req);
 
  2649 
  2654 
  2650 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
  2655 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
  2651         uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
 
  2656         uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
 
  2652         uint16_t *error_code)
 
  2657         uint16_t *error_code)
 
  2653 {
 
  2658 {
 
  2654     ec_master_soe_request_t* request;
 
  2659     ec_master_soe_request_t *request;
 
  2655     int retval;
 
  2660     int retval;
 
  2656 
  2661 
  2657     if (drive_no > 7) {
 
  2662     if (drive_no > 7) {
 
  2658         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2663         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2659         return -EINVAL;
 
  2664         return -EINVAL;
 
  2660     }
 
  2665     }
 
  2661 
  2666 
  2662     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2667     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2663     if (!request)
 
  2668     if (!request) {
 
  2664         return -ENOMEM;
 
  2669         return -ENOMEM;
 
       
  2670     }
 
  2665     kref_init(&request->refcount);
 
  2671     kref_init(&request->refcount);
 
  2666 
  2672 
  2667     INIT_LIST_HEAD(&request->list);
 
  2673     INIT_LIST_HEAD(&request->list);
 
  2668     ec_soe_request_init(&request->req);
 
  2674     ec_soe_request_init(&request->req);
 
  2669     ec_soe_request_set_drive_no(&request->req, drive_no);
 
  2675     ec_soe_request_set_drive_no(&request->req, drive_no);
 
  2724 
  2730 
  2725 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
 
  2731 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
 
  2726         uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
 
  2732         uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
 
  2727         size_t *result_size, uint16_t *error_code)
 
  2733         size_t *result_size, uint16_t *error_code)
 
  2728 {
 
  2734 {
 
  2729     ec_master_soe_request_t* request;
 
  2735     ec_master_soe_request_t *request;
 
  2730 
  2736 
  2731     if (drive_no > 7) {
 
  2737     if (drive_no > 7) {
 
  2732         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2738         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2733         return -EINVAL;
 
  2739         return -EINVAL;
 
  2734     }
 
  2740     }
 
  2735 
  2741 
  2736     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2742     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2737     if (!request)
 
  2743     if (!request) {
 
  2738         return -ENOMEM;
 
  2744         return -ENOMEM;
 
       
  2745     }
 
  2739     kref_init(&request->refcount);
 
  2746     kref_init(&request->refcount);
 
  2740 
  2747 
  2741     INIT_LIST_HEAD(&request->list);
 
  2748     INIT_LIST_HEAD(&request->list);
 
  2742     ec_soe_request_init(&request->req);
 
  2749     ec_soe_request_init(&request->req);
 
  2743     ec_soe_request_set_drive_no(&request->req, drive_no);
 
  2750     ec_soe_request_set_drive_no(&request->req, drive_no);
 
  2839 EXPORT_SYMBOL(ecrt_master_sdo_upload);
 
  2846 EXPORT_SYMBOL(ecrt_master_sdo_upload);
 
  2840 EXPORT_SYMBOL(ecrt_master_write_idn);
 
  2847 EXPORT_SYMBOL(ecrt_master_write_idn);
 
  2841 EXPORT_SYMBOL(ecrt_master_read_idn);
 
  2848 EXPORT_SYMBOL(ecrt_master_read_idn);
 
  2842 EXPORT_SYMBOL(ecrt_master_reset);
 
  2849 EXPORT_SYMBOL(ecrt_master_reset);
 
  2843 EXPORT_SYMBOL(ecrt_master_find_domain);
 
  2850 EXPORT_SYMBOL(ecrt_master_find_domain);
 
       
  2851 
  2844 /** \endcond */
 
  2852 /** \endcond */
 
  2845 
  2853 
  2846 /*****************************************************************************/
 
  2854 /*****************************************************************************/
etherlabmaster