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 2107 ebda087981e1
parent 2103 8c63d804ead9
child 2110 455126b35650
equal deleted inserted replaced
2106:505871ced767 2107:ebda087981e1 
   416         request->state = EC_INT_REQUEST_FAILURE;
 
   416         request->state = EC_INT_REQUEST_FAILURE;
 
   417         kref_put(&request->refcount,ec_master_reg_request_release);
 
   417         kref_put(&request->refcount,ec_master_reg_request_release);
 
   418         wake_up(&master->reg_queue);
 
   418         wake_up(&master->reg_queue);
 
   419     }
 
   419     }
 
   420 
   420 
   421     // we must lock the io_mutex here because the slave's fsm_datagram will be unqueued
 
   421     // we must lock the io_mutex here because the slave's fsm_datagram
 
       
   422     // will be unqueued
 
   422     ec_mutex_lock(&master->io_mutex);
 
   423     ec_mutex_lock(&master->io_mutex);
 
   423     for (slave = master->slaves;
 
   424     for (slave = master->slaves;
 
   424             slave < master->slaves + master->slave_count;
 
   425             slave < master->slaves + master->slave_count;
 
   425             slave++) {
 
   426             slave++) {
 
   426         ec_slave_clear(slave);
 
   427         ec_slave_clear(slave);
 
   441  */
 
   442  */
 
   442 void ec_master_clear_domains(ec_master_t *master)
 
   443 void ec_master_clear_domains(ec_master_t *master)
 
   443 {
 
   444 {
 
   444     ec_domain_t *domain, *next;
 
   445     ec_domain_t *domain, *next;
 
   445 
   446 
   446     // we must lock the io_mutex here because the domains's datagram will be unqueued
 
   447     // we must lock the io_mutex here because the domains's datagram
 
       
   448     // will be unqueued
 
   447     ec_mutex_lock(&master->io_mutex);
 
   449     ec_mutex_lock(&master->io_mutex);
 
   448     list_for_each_entry_safe(domain, next, &master->domains, list) {
 
   450     list_for_each_entry_safe(domain, next, &master->domains, list) {
 
   449         list_del(&domain->list);
 
   451         list_del(&domain->list);
 
   450         ec_domain_clear(domain);
 
   452         ec_domain_clear(domain);
 
   451         kfree(domain);
 
   453         kfree(domain);
 
   566 
   568 
   567 /*****************************************************************************/
 
   569 /*****************************************************************************/
 
   568 
   570 
   569 /** Transition function from IDLE to OPERATION phase.
 
   571 /** Transition function from IDLE to OPERATION phase.
 
   570  */
 
   572  */
 
   571 int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */)
 
   573 int ec_master_enter_operation_phase(i
 
       
   574         ec_master_t *master /**< EtherCAT master */
 
       
   575         )
 
   572 {
 
   576 {
 
   573     int ret = 0;
 
   577     int ret = 0;
 
   574     ec_slave_t *slave;
 
   578     ec_slave_t *slave;
 
   575 #ifdef EC_EOE
 
   579 #ifdef EC_EOE
 
   576     ec_eoe_t *eoe;
 
   580     ec_eoe_t *eoe;
 
  1938     unsigned int index;
 
  1942     unsigned int index;
 
  1939 
  1943 
  1940     EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n",
 
  1944     EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n",
 
  1941             master);
 
  1945             master);
 
  1942 
  1946 
  1943     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
  1947     if (!(domain =
 
       
  1948                 (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
  1944         EC_MASTER_ERR(master, "Error allocating domain memory!\n");
 
  1949         EC_MASTER_ERR(master, "Error allocating domain memory!\n");
 
  1945         return ERR_PTR(-ENOMEM);
 
  1950         return ERR_PTR(-ENOMEM);
 
  1946     }
 
  1951     }
 
  1947 
  1952 
  1948     ec_mutex_lock(&master->master_mutex);
 
  1953     ec_mutex_lock(&master->master_mutex);
 
  2073                 is_eoe_slave = 1;
 
  2078                 is_eoe_slave = 1;
 
  2074        }
 
  2079        }
 
  2075        if (!is_eoe_slave) {
 
  2080        if (!is_eoe_slave) {
 
  2076            ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
  2081            ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
  2077            // mark for reconfiguration, because the master could have no
 
  2082            // mark for reconfiguration, because the master could have no
 
  2078            // possibility for a reconfiguration between two sequential operation
 
  2083            // possibility for a reconfiguration between two sequential
 
  2079            // phases.
 
  2084            // operation phases.
 
  2080            slave->force_config = 1;
 
  2085            slave->force_config = 1;
 
  2081         }
 
  2086         }
 
  2082 #else
 
  2087 #else
 
  2083         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
  2088         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
  2084         // mark for reconfiguration, because the master could have no
 
  2089         // mark for reconfiguration, because the master could have no
 
  2318     state->link_up = master->main_device.link_state;
 
  2323     state->link_up = master->main_device.link_state;
 
  2319 }
 
  2324 }
 
  2320 
  2325 
  2321 /*****************************************************************************/
 
  2326 /*****************************************************************************/
 
  2322 
  2327 
  2323 void ecrt_master_configured_slaves_state(const ec_master_t *master, ec_master_state_t *state)
 
  2328 void ecrt_master_configured_slaves_state(
 
       
  2329         const ec_master_t *master,
 
       
  2330         ec_master_state_t *state
 
       
  2331         )
 
  2324 {
 
  2332 {
 
  2325     const ec_slave_config_t *sc;
 
  2333     const ec_slave_config_t *sc;
 
  2326     ec_slave_config_state_t sc_state;
 
  2334     ec_slave_config_state_t sc_state;
 
  2327 
  2335 
  2328     // collect al_states of all configured online slaves
 
  2336     // collect al_states of all configured online slaves
 
  2342 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 
  2350 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 
  2343 {
 
  2351 {
 
  2344     master->app_time = app_time;
 
  2352     master->app_time = app_time;
 
  2345 
  2353 
  2346     if (unlikely(!master->has_app_time)) {
 
  2354     if (unlikely(!master->has_app_time)) {
 
  2347 		EC_MASTER_DBG(master, 1, "set application start time = %llu\n",app_time);
 
  2355 		EC_MASTER_DBG(master, 1, "Set application start time = %llu\n",
 
  2348 		master->app_start_time = app_time;
 
  2356                 app_time);
 
       
  2357         master->app_start_time = app_time;
 
  2349 #ifdef EC_HAVE_CYCLES
 
  2358 #ifdef EC_HAVE_CYCLES
 
  2350     master->dc_cycles_app_start_time = get_cycles();
 
  2359         master->dc_cycles_app_start_time = get_cycles();
 
  2351 #endif
 
  2360 #endif
 
  2352     master->dc_jiffies_app_start_time = jiffies;
 
  2361         master->dc_jiffies_app_start_time = jiffies;
 
  2353         master->has_app_time = 1;
 
  2362         master->has_app_time = 1;
 
  2354     }
 
  2363     }
 
  2355 }
 
  2364 }
 
  2356 
  2365 
  2357 /*****************************************************************************/
 
  2366 /*****************************************************************************/
 
  2385     if (master->sync_mon_datagram.state == EC_DATAGRAM_RECEIVED) {
 
  2394     if (master->sync_mon_datagram.state == EC_DATAGRAM_RECEIVED) {
 
  2386         return EC_READ_U32(master->sync_mon_datagram.data) & 0x7fffffff;
 
  2395         return EC_READ_U32(master->sync_mon_datagram.data) & 0x7fffffff;
 
  2387     } else {
 
  2396     } else {
 
  2388         return 0xffffffff;
 
  2397         return 0xffffffff;
 
  2389     }
 
  2398     }
 
       
  2399 }
 
       
  2400 
       
  2401 /*****************************************************************************/
 
       
  2402 
       
  2403 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
 
       
  2404         uint16_t index, uint8_t subindex, uint8_t *data,
 
       
  2405         size_t data_size, uint32_t *abort_code)
 
       
  2406 {
 
       
  2407     ec_master_sdo_request_t* request;
 
       
  2408     int retval;
 
       
  2409 
       
  2410     if (!data_size) {
 
       
  2411         EC_MASTER_ERR(master, "Zero data size!\n");
 
       
  2412         return -EINVAL;
 
       
  2413     }
 
       
  2414 
       
  2415     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
       
  2416     if (!request) {
 
       
  2417         return -ENOMEM;
 
       
  2418     }
 
       
  2419     kref_init(&request->refcount);
 
       
  2420 
       
  2421     ec_sdo_request_init(&request->req);
 
       
  2422     ec_sdo_request_address(&request->req, index, subindex);
 
       
  2423     if (ec_sdo_request_alloc(&request->req, data_size)) {
 
       
  2424         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2425         return -ENOMEM;
 
       
  2426     }
 
       
  2427     memcpy(request->req.data, data, data_size);
 
       
  2428     request->req.data_size = data_size;
 
       
  2429     ecrt_sdo_request_write(&request->req);
 
       
  2430 
       
  2431     if (ec_mutex_lock_interruptible(&master->master_mutex)) {
 
       
  2432         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2433         return -EINTR;
 
       
  2434     }
 
       
  2435 
       
  2436     if (!(request->slave = ec_master_find_slave(
 
       
  2437                     master, 0, slave_position))) {
 
       
  2438         ec_mutex_unlock(&master->master_mutex);
 
       
  2439         EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
 
       
  2440         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2441         return -EINVAL;
 
       
  2442     }
 
       
  2443     
       
  2444     EC_SLAVE_DBG(request->slave, 1, "Schedule SDO download request %p.\n",
 
       
  2445             request);
 
       
  2446 
       
  2447     // schedule request
 
       
  2448     kref_get(&request->refcount);
 
       
  2449     list_add_tail(&request->list, &request->slave->slave_sdo_requests);
 
       
  2450 
       
  2451     ec_mutex_unlock(&master->master_mutex);
 
       
  2452 
       
  2453     // wait for processing through FSM
 
       
  2454     if (wait_event_interruptible(request->slave->sdo_queue,
 
       
  2455        ((request->req.state == EC_INT_REQUEST_SUCCESS) ||
 
       
  2456         (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
       
  2457         // interrupted by signal
 
       
  2458         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2459         return -EINTR;
 
       
  2460     }
 
       
  2461 
       
  2462     EC_SLAVE_DBG(request->slave, 1, "Finished SDO download request %p.\n",
 
       
  2463             request);
 
       
  2464 
       
  2465     *abort_code = request->req.abort_code;
 
       
  2466 
       
  2467     if (request->req.state == EC_INT_REQUEST_SUCCESS) {
 
       
  2468         retval = 0;
 
       
  2469     } else if (request->req.errno) {
 
       
  2470         retval = -request->req.errno;
 
       
  2471     } else {
 
       
  2472         retval = -EIO;
 
       
  2473     }
 
       
  2474 
       
  2475     kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2476     return retval;
 
       
  2477 }
 
       
  2478 
       
  2479 /*****************************************************************************/
 
       
  2480 
       
  2481 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
 
       
  2482         uint16_t index, uint8_t subindex, uint8_t *target,
 
       
  2483         size_t target_size, size_t *result_size, uint32_t *abort_code)
 
       
  2484 {
 
       
  2485     ec_master_sdo_request_t* request;
 
       
  2486     int retval;
 
       
  2487 
       
  2488     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
       
  2489     if (!request)
 
       
  2490         return -ENOMEM;
 
       
  2491     kref_init(&request->refcount);
 
       
  2492 
       
  2493     ec_sdo_request_init(&request->req);
 
       
  2494     ec_sdo_request_address(&request->req, index, subindex);
 
       
  2495     ecrt_sdo_request_read(&request->req);
 
       
  2496 
       
  2497     if (ec_mutex_lock_interruptible(&master->master_mutex))  {
 
       
  2498         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2499         return -EINTR;
 
       
  2500     }
 
       
  2501 
       
  2502     if (!(request->slave = ec_master_find_slave(
 
       
  2503                     master, 0, slave_position))) {
 
       
  2504         ec_mutex_unlock(&master->master_mutex);
 
       
  2505         EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
 
       
  2506         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2507         return -EINVAL;
 
       
  2508     }
 
       
  2509 
       
  2510     EC_SLAVE_DBG(request->slave, 1, "Schedule SDO upload request %p.\n",
 
       
  2511             request);
 
       
  2512 
       
  2513     // schedule request
 
       
  2514     kref_get(&request->refcount);
 
       
  2515     list_add_tail(&request->list, &request->slave->slave_sdo_requests);
 
       
  2516 
       
  2517     ec_mutex_unlock(&master->master_mutex);
 
       
  2518 
       
  2519     // wait for processing through FSM
 
       
  2520     if (wait_event_interruptible(request->slave->sdo_queue,
 
       
  2521           ((request->req.state == EC_INT_REQUEST_SUCCESS) ||
 
       
  2522            (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
       
  2523         // interrupted by signal
 
       
  2524         kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2525         return -EINTR;
 
       
  2526     }
 
       
  2527 
       
  2528     EC_SLAVE_DBG(request->slave, 1, "Finished SDO upload request %p.\n",
 
       
  2529             request);
 
       
  2530 
       
  2531     *abort_code = request->req.abort_code;
 
       
  2532 
       
  2533     if (request->req.state != EC_INT_REQUEST_SUCCESS) {
 
       
  2534         *result_size = 0;
 
       
  2535         if (request->req.errno) {
 
       
  2536             retval = -request->req.errno;
 
       
  2537         } else {
 
       
  2538             retval = -EIO;
 
       
  2539         }
 
       
  2540     } else {
 
       
  2541         if (request->req.data_size > target_size) {
 
       
  2542             EC_MASTER_ERR(master, "Buffer too small.\n");
 
       
  2543             kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2544             return -EOVERFLOW;
 
       
  2545         }
 
       
  2546         memcpy(target, request->req.data, request->req.data_size);
 
       
  2547         *result_size = request->req.data_size;
 
       
  2548         retval = 0;
 
       
  2549     }
 
       
  2550 
       
  2551     kref_put(&request->refcount, ec_master_sdo_request_release);
 
       
  2552     return retval;
 
  2390 }
 
  2553 }
 
  2391 
  2554 
  2392 /*****************************************************************************/
 
  2555 /*****************************************************************************/
 
  2393 
  2556 
  2394 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
  2557 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
  2435                 slave_position);
 
  2598                 slave_position);
 
  2436         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2599         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2437         return -EINVAL;
 
  2600         return -EINVAL;
 
  2438     }
 
  2601     }
 
  2439 
  2602 
  2440     EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE write request %p.\n",request);
 
  2603     EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE write request %p.\n",
 
       
  2604             request);
 
  2441 
  2605 
  2442     // schedule SoE write request.
 
  2606     // schedule SoE write request.
 
  2443     list_add_tail(&request->list, &request->slave->soe_requests);
 
  2607     list_add_tail(&request->list, &request->slave->soe_requests);
 
  2444     kref_get(&request->refcount);
 
  2608     kref_get(&request->refcount);
 
  2445 
  2609 
  2446     ec_mutex_unlock(&master->master_mutex);
 
  2610     ec_mutex_unlock(&master->master_mutex);
 
  2447 
  2611 
  2448     // wait for processing through FSM
 
  2612     // wait for processing through FSM
 
  2449     if (wait_event_interruptible(request->slave->soe_queue,
 
  2613     if (wait_event_interruptible(request->slave->soe_queue,
 
  2450           ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2614           ((request->req.state == EC_INT_REQUEST_SUCCESS) ||
 
       
  2615            (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2451            // interrupted by signal
 
  2616            // interrupted by signal
 
  2452            kref_put(&request->refcount,ec_master_soe_request_release);
 
  2617            kref_put(&request->refcount,ec_master_soe_request_release);
 
  2453            return -EINTR;
 
  2618            return -EINTR;
 
  2454     }
 
  2619     }
 
  2455 
  2620 
  2502     list_add_tail(&request->list, &request->slave->soe_requests);
 
  2667     list_add_tail(&request->list, &request->slave->soe_requests);
 
  2503     kref_get(&request->refcount);
 
  2668     kref_get(&request->refcount);
 
  2504 
  2669 
  2505     ec_mutex_unlock(&master->master_mutex);
 
  2670     ec_mutex_unlock(&master->master_mutex);
 
  2506 
  2671 
  2507     EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE read request %p.\n",request);
 
  2672     EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE read request %p.\n",
 
       
  2673             request);
 
  2508 
  2674 
  2509     // wait for processing through FSM
 
  2675     // wait for processing through FSM
 
  2510     if (wait_event_interruptible(request->slave->soe_queue,
 
  2676     if (wait_event_interruptible(request->slave->soe_queue,
 
  2511           ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2677           ((request->req.state == EC_INT_REQUEST_SUCCESS) ||
 
       
  2678            (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2512            // interrupted by signal
 
  2679            // interrupted by signal
 
  2513            kref_put(&request->refcount,ec_master_soe_request_release);
 
  2680            kref_put(&request->refcount,ec_master_soe_request_release);
 
  2514            return -EINTR;
 
  2681            return -EINTR;
 
  2515     }
 
  2682     }
 
  2516 
  2683 
  2517     if (error_code) {
 
  2684     if (error_code) {
 
  2518         *error_code = request->req.error_code;
 
  2685         *error_code = request->req.error_code;
 
  2519     }
 
  2686     }
 
  2520 
  2687 
  2521     EC_SLAVE_DBG(request->slave, 1, "SoE request %p read %zd bytes via SoE.\n",
 
  2688     EC_SLAVE_DBG(request->slave, 1, "SoE request %p read %zd bytes"
 
  2522             request,request->req.data_size);
 
  2689             " via SoE.\n", request, request->req.data_size);
 
  2523 
  2690 
  2524     if (request->req.state != EC_INT_REQUEST_SUCCESS) {
 
  2691     if (request->req.state != EC_INT_REQUEST_SUCCESS) {
 
  2525         if (result_size) {
 
  2692         if (result_size) {
 
  2526             *result_size = 0;
 
  2693             *result_size = 0;
 
  2527         }
 
  2694         }
 
  2572 EXPORT_SYMBOL(ecrt_master_application_time);
 
  2739 EXPORT_SYMBOL(ecrt_master_application_time);
 
  2573 EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
 
  2740 EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
 
  2574 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
 
  2741 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
 
  2575 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
 
  2742 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
 
  2576 EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
 
  2743 EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
 
       
  2744 EXPORT_SYMBOL(ecrt_master_sdo_download);
 
       
  2745 EXPORT_SYMBOL(ecrt_master_sdo_upload);
 
  2577 EXPORT_SYMBOL(ecrt_master_write_idn);
 
  2746 EXPORT_SYMBOL(ecrt_master_write_idn);
 
  2578 EXPORT_SYMBOL(ecrt_master_read_idn);
 
  2747 EXPORT_SYMBOL(ecrt_master_read_idn);
 
  2579 EXPORT_SYMBOL(ecrt_master_reset);
 
  2748 EXPORT_SYMBOL(ecrt_master_reset);
 
  2580 EXPORT_SYMBOL(ecrt_master_find_domain);
 
  2749 EXPORT_SYMBOL(ecrt_master_find_domain);
 
  2581 /** \endcond */
 
  2750 /** \endcond */
etherlabmaster