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 2060 8b67602f5161
parent 2054 3417bbc4ad2f
parent 2046 4cf0161c445a
child 2080 42fbd117c3e3
equal deleted inserted replaced
2059:ab0b96ac18bb 2060:8b67602f5161 
    54 #endif
 
    54 #endif
 
    55 #include "master.h"
 
    55 #include "master.h"
 
    56 
    56 
    57 /*****************************************************************************/
 
    57 /*****************************************************************************/
 
    58 
    58 
    59 /** Set to 1 to enable external datagram injection debugging.
 
    59 /** Set to 1 to enable fsm datagram injection debugging.
 
    60  */
 
    60  */
 
       
    61 #ifdef USE_TRACE_PRINTK
 
       
    62 #define DEBUG_INJECT 1
 
       
    63 #else
 
    61 #define DEBUG_INJECT 0
 
    64 #define DEBUG_INJECT 0
 
       
    65 #endif
 
    62 
    66 
    63 #ifdef EC_HAVE_CYCLES
 
    67 #ifdef EC_HAVE_CYCLES
 
    64 
    68 
    65 /** Frame timeout in cycles.
 
    69 /** Frame timeout in cycles.
 
    66  */
 
    70  */
 
    67 static cycles_t timeout_cycles;
 
    71 static cycles_t timeout_cycles;
 
    68 
    72 
    69 /** Timeout for external datagram injection [cycles].
 
    73 /** Timeout for fsm datagram injection [cycles].
 
    70  */
 
    74  */
 
    71 static cycles_t ext_injection_timeout_cycles;
 
    75 static cycles_t fsm_injection_timeout_cycles;
 
    72 
    76 
    73 #else
 
    77 #else
 
    74 
    78 
    75 /** Frame timeout in jiffies.
 
    79 /** Frame timeout in jiffies.
 
    76  */
 
    80  */
 
    77 static unsigned long timeout_jiffies;
 
    81 static unsigned long timeout_jiffies;
 
    78 
    82 
    79 /** Timeout for external datagram injection [jiffies].
 
    83 /** Timeout for fsm datagram injection [jiffies].
 
    80  */
 
    84  */
 
    81 static unsigned long ext_injection_timeout_jiffies;
 
    85 static unsigned long fsm_injection_timeout_jiffies;
 
    82 
    86 
    83 #endif
 
    87 #endif
 
    84 
    88 
    85 /*****************************************************************************/
 
    89 /*****************************************************************************/
 
    86 
    90 
    87 void ec_master_clear_slave_configs(ec_master_t *);
 
    91 void ec_master_clear_slave_configs(ec_master_t *);
 
    88 void ec_master_clear_domains(ec_master_t *);
 
    92 void ec_master_clear_domains(ec_master_t *);
 
    89 static int ec_master_idle_thread(void *);
 
    93 static int ec_master_idle_thread(void *);
 
    90 static int ec_master_operation_thread(void *);
 
    94 static int ec_master_operation_thread(void *);
 
    91 #ifdef EC_EOE
 
    95 #ifdef EC_EOE
 
    92 static int ec_master_eoe_thread(void *);
 
    96 static int ec_master_eoe_processing(ec_master_t *);
 
    93 #endif
 
    97 #endif
 
    94 void ec_master_find_dc_ref_clock(ec_master_t *);
 
    98 void ec_master_find_dc_ref_clock(ec_master_t *);
 
    95 
    99 
    96 /*****************************************************************************/
 
   100 /*****************************************************************************/
 
    97 
   101 
    99 */
 
   103 */
 
   100 void ec_master_init_static(void)
 
   104 void ec_master_init_static(void)
 
   101 {
 
   105 {
 
   102 #ifdef EC_HAVE_CYCLES
 
   106 #ifdef EC_HAVE_CYCLES
 
   103     timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
 
   107     timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
 
   104     ext_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
 
   108     fsm_injection_timeout_cycles = (cycles_t) EC_FSM_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
 
   105 #else
 
   109 #else
 
   106     // one jiffy may always elapse between time measurement
 
   110     // one jiffy may always elapse between time measurement
 
   107     timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
 
   111     timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
 
   108     ext_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
 
   112     fsm_injection_timeout_jiffies = max(EC_FSM_INJECTION_TIMEOUT * HZ / 1000000, 1);
 
   109 #endif
 
   113 #endif
 
   110 }
 
   114 }
 
   111 
   115 
   112 /*****************************************************************************/
 
   116 /*****************************************************************************/
 
   113 
   117 
   128     int ret;
 
   132     int ret;
 
   129 
   133 
   130     master->index = index;
 
   134     master->index = index;
 
   131     master->reserved = 0;
 
   135     master->reserved = 0;
 
   132 
   136 
   133     sema_init(&master->master_sem, 1);
 
   137     ec_mutex_init(&master->master_mutex);
 
   134 
   138 
   135     master->main_mac = main_mac;
 
   139     master->main_mac = main_mac;
 
   136     master->backup_mac = backup_mac;
 
   140     master->backup_mac = backup_mac;
 
   137 
   141 
   138     sema_init(&master->device_sem, 1);
 
   142     ec_mutex_init(&master->device_mutex);
 
   139 
   143 
   140     master->phase = EC_ORPHANED;
 
   144     master->phase = EC_ORPHANED;
 
   141     master->active = 0;
 
   145     master->active = 0;
 
   142     master->config_changed = 0;
 
   146     master->config_changed = 0;
 
   143     master->injection_seq_fsm = 0;
 
   147     master->injection_seq_fsm = 0;
 
   144     master->injection_seq_rt = 0;
 
   148     master->injection_seq_rt = 0;
 
   145 
   149 
   146     master->slaves = NULL;
 
   150     master->slaves = NULL;
 
   147     master->slave_count = 0;
 
   151     master->slave_count = 0;
 
   148     
   152 
   149     INIT_LIST_HEAD(&master->configs);
 
   153     INIT_LIST_HEAD(&master->configs);
 
   150 
   154 
   151     master->app_time = 0ULL;
 
   155     master->app_time = 0ULL;
 
       
   156 #ifdef EC_HAVE_CYCLES
 
       
   157     master->dc_cycles_app_start_time = 0;
 
       
   158 #endif
 
       
   159     master->dc_jiffies_app_start_time = 0;
 
   152     master->app_start_time = 0ULL;
 
   160     master->app_start_time = 0ULL;
 
   153     master->has_app_time = 0;
 
   161     master->has_app_time = 0;
 
   154 
   162 
   155     master->scan_busy = 0;
 
   163     master->scan_busy = 0;
 
   156     master->allow_scan = 1;
 
   164     master->allow_scan = 1;
 
   157     sema_init(&master->scan_sem, 1);
 
   165     ec_mutex_init(&master->scan_mutex);
 
   158     init_waitqueue_head(&master->scan_queue);
 
   166     init_waitqueue_head(&master->scan_queue);
 
   159 
   167 
   160     master->config_busy = 0;
 
   168     master->config_busy = 0;
 
   161     master->allow_config = 1;
 
   169     master->allow_config = 1;
 
   162     sema_init(&master->config_sem, 1);
 
   170     ec_mutex_init(&master->config_mutex);
 
   163     init_waitqueue_head(&master->config_queue);
 
   171     init_waitqueue_head(&master->config_queue);
 
   164     
   172     
   165     INIT_LIST_HEAD(&master->datagram_queue);
 
   173     INIT_LIST_HEAD(&master->datagram_queue);
 
   166     master->datagram_index = 0;
 
   174     master->datagram_index = 0;
 
   167 
   175 
   168     INIT_LIST_HEAD(&master->ext_datagram_queue);
 
   176     ec_mutex_init(&master->fsm_queue_mutex);
 
   169     sema_init(&master->ext_queue_sem, 1);
 
   177     INIT_LIST_HEAD(&master->fsm_datagram_queue);
 
   170 
       
   171     INIT_LIST_HEAD(&master->external_datagram_queue);
 
       
   172     
   178     
   173     // send interval in IDLE phase
 
   179     // send interval in IDLE phase
 
   174     ec_master_set_send_interval(master, 1000000 / HZ);
 
   180     ec_master_set_send_interval(master, 1000000 / HZ);
 
   175 
   181 
   176     INIT_LIST_HEAD(&master->domains);
 
   182     INIT_LIST_HEAD(&master->domains);
 
   186 #ifdef EC_EOE
 
   192 #ifdef EC_EOE
 
   187     master->eoe_thread = NULL;
 
   193     master->eoe_thread = NULL;
 
   188     INIT_LIST_HEAD(&master->eoe_handlers);
 
   194     INIT_LIST_HEAD(&master->eoe_handlers);
 
   189 #endif
 
   195 #endif
 
   190 
   196 
   191     sema_init(&master->io_sem, 1);
 
   197     ec_mutex_init(&master->io_mutex);
 
   192     master->send_cb = NULL;
 
   198     master->fsm_queue_lock_cb = NULL;
 
   193     master->receive_cb = NULL;
 
   199     master->fsm_queue_unlock_cb = NULL;
 
   194     master->cb_data = NULL;
 
   200     master->fsm_queue_locking_data = NULL;
 
   195     master->app_send_cb = NULL;
 
   201     master->app_fsm_queue_lock_cb = NULL;
 
   196     master->app_receive_cb = NULL;
 
   202     master->app_fsm_queue_unlock_cb = NULL;
 
   197     master->app_cb_data = NULL;
 
   203     master->app_fsm_queue_locking_data = NULL;
 
   198 
   204 
   199     INIT_LIST_HEAD(&master->sii_requests);
 
   205     INIT_LIST_HEAD(&master->sii_requests);
 
   200     init_waitqueue_head(&master->sii_queue);
 
   206     init_waitqueue_head(&master->sii_queue);
 
   201 
   207 
   202     INIT_LIST_HEAD(&master->reg_requests);
 
   208     INIT_LIST_HEAD(&master->reg_requests);
 
   220         EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n");
 
   226         EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n");
 
   221         goto out_clear_backup;
 
   227         goto out_clear_backup;
 
   222     }
 
   228     }
 
   223 
   229 
   224     // create state machine object
 
   230     // create state machine object
 
       
   231     ec_mbox_init(&master->fsm_mbox,&master->fsm_datagram);
 
   225     ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
 
   232     ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
 
   226 
   233 
   227     // init reference sync datagram
 
   234     // init reference sync datagram
 
   228     ec_datagram_init(&master->ref_sync_datagram);
 
   235     ec_datagram_init(&master->ref_sync_datagram);
 
   229     snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync");
 
   236     snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync");
 
   321 #else
 
   328 #else
 
   322     class_device_unregister(master->class_device);
 
   329     class_device_unregister(master->class_device);
 
   323 #endif
 
   330 #endif
 
   324 
   331 
   325     ec_cdev_clear(&master->cdev);
 
   332     ec_cdev_clear(&master->cdev);
 
   326     
   333 
   327 #ifdef EC_EOE
 
   334 #ifdef EC_EOE
 
   328     ec_master_clear_eoe_handlers(master);
 
   335     ec_master_clear_eoe_handlers(master);
 
   329 #endif
 
   336 #endif
 
   330     ec_master_clear_domains(master);
 
   337     ec_master_clear_domains(master);
 
   331     ec_master_clear_slave_configs(master);
 
   338     ec_master_clear_slave_configs(master);
 
   333 
   340 
   334     ec_datagram_clear(&master->sync_mon_datagram);
 
   341     ec_datagram_clear(&master->sync_mon_datagram);
 
   335     ec_datagram_clear(&master->sync_datagram);
 
   342     ec_datagram_clear(&master->sync_datagram);
 
   336     ec_datagram_clear(&master->ref_sync_datagram);
 
   343     ec_datagram_clear(&master->ref_sync_datagram);
 
   337     ec_fsm_master_clear(&master->fsm);
 
   344     ec_fsm_master_clear(&master->fsm);
 
       
   345     ec_mbox_clear(&master->fsm_mbox);
 
   338     ec_datagram_clear(&master->fsm_datagram);
 
   346     ec_datagram_clear(&master->fsm_datagram);
 
   339     ec_device_clear(&master->backup_device);
 
   347     ec_device_clear(&master->backup_device);
 
   340     ec_device_clear(&master->main_device);
 
   348     ec_device_clear(&master->main_device);
 
   341 }
 
   349 }
 
   342 
   350 
   393                     ec_sii_write_request_t, list);
 
   401                     ec_sii_write_request_t, list);
 
   394         list_del_init(&request->list); // dequeue
 
   402         list_del_init(&request->list); // dequeue
 
   395         EC_MASTER_WARN(master, "Discarding SII request, slave %u about"
 
   403         EC_MASTER_WARN(master, "Discarding SII request, slave %u about"
 
   396                 " to be deleted.\n", request->slave->ring_position);
 
   404                 " to be deleted.\n", request->slave->ring_position);
 
   397         request->state = EC_INT_REQUEST_FAILURE;
 
   405         request->state = EC_INT_REQUEST_FAILURE;
 
       
   406         kref_put(&request->refcount,ec_master_sii_write_request_release);
 
   398         wake_up(&master->sii_queue);
 
   407         wake_up(&master->sii_queue);
 
   399     }
 
   408     }
 
   400 
   409 
   401     while (!list_empty(&master->reg_requests)) {
 
   410     while (!list_empty(&master->reg_requests)) {
 
   402         ec_reg_request_t *request =
 
   411         ec_reg_request_t *request =
 
   403             list_entry(master->reg_requests.next, ec_reg_request_t, list);
 
   412             list_entry(master->reg_requests.next, ec_reg_request_t, list);
 
   404         list_del_init(&request->list); // dequeue
 
   413         list_del_init(&request->list); // dequeue
 
   405         EC_MASTER_WARN(master, "Discarding register request, slave %u"
 
   414         EC_MASTER_WARN(master, "Discarding register request, slave %u"
 
   406                 " about to be deleted.\n", request->slave->ring_position);
 
   415                 " about to be deleted.\n", request->slave->ring_position);
 
   407         request->state = EC_INT_REQUEST_FAILURE;
 
   416         request->state = EC_INT_REQUEST_FAILURE;
 
       
   417         kref_put(&request->refcount,ec_master_reg_request_release);
 
   408         wake_up(&master->reg_queue);
 
   418         wake_up(&master->reg_queue);
 
   409     }
 
   419     }
 
   410 
   420 
       
   421     // we must lock the io_mutex here because the slave's fsm_datagram will be unqueued
 
       
   422     ec_mutex_lock(&master->io_mutex);
 
   411     for (slave = master->slaves;
 
   423     for (slave = master->slaves;
 
   412             slave < master->slaves + master->slave_count;
 
   424             slave < master->slaves + master->slave_count;
 
   413             slave++) {
 
   425             slave++) {
 
   414         ec_slave_clear(slave);
 
   426         ec_slave_clear(slave);
 
   415     }
 
   427     }
 
       
   428     ec_mutex_unlock(&master->io_mutex);
 
   416 
   429 
   417     if (master->slaves) {
 
   430     if (master->slaves) {
 
   418         kfree(master->slaves);
 
   431         kfree(master->slaves);
 
   419         master->slaves = NULL;
 
   432         master->slaves = NULL;
 
   420     }
 
   433     }
 
   428  */
 
   441  */
 
   429 void ec_master_clear_domains(ec_master_t *master)
 
   442 void ec_master_clear_domains(ec_master_t *master)
 
   430 {
 
   443 {
 
   431     ec_domain_t *domain, *next;
 
   444     ec_domain_t *domain, *next;
 
   432 
   445 
       
   446     // we must lock the io_mutex here because the domains's datagram will be unqueued
 
       
   447     ec_mutex_lock(&master->io_mutex);
 
   433     list_for_each_entry_safe(domain, next, &master->domains, list) {
 
   448     list_for_each_entry_safe(domain, next, &master->domains, list) {
 
   434         list_del(&domain->list);
 
   449         list_del(&domain->list);
 
   435         ec_domain_clear(domain);
 
   450         ec_domain_clear(domain);
 
   436         kfree(domain);
 
   451         kfree(domain);
 
   437     }
 
   452     }
 
       
   453     ec_mutex_unlock(&master->io_mutex);
 
   438 }
 
   454 }
 
   439 
   455 
   440 /*****************************************************************************/
 
   456 /*****************************************************************************/
 
   441 
   457 
   442 /** Clear the configuration applied by the application.
 
   458 /** Clear the configuration applied by the application.
 
   443  */
 
   459  */
 
   444 void ec_master_clear_config(
 
   460 void ec_master_clear_config(
 
   445         ec_master_t *master /**< EtherCAT master. */
 
   461         ec_master_t *master /**< EtherCAT master. */
 
   446         )
 
   462         )
 
   447 {
 
   463 {
 
   448     down(&master->master_sem);
 
   464     ec_mutex_lock(&master->master_mutex);
 
   449     ec_master_clear_domains(master);
 
   465     ec_master_clear_domains(master);
 
   450     ec_master_clear_slave_configs(master);
 
   466     ec_master_clear_slave_configs(master);
 
   451     up(&master->master_sem);
 
   467     ec_mutex_unlock(&master->master_mutex);
 
   452 }
 
       
   453 
       
   454 /*****************************************************************************/
 
       
   455 
       
   456 /** Internal sending callback.
 
       
   457  */
 
       
   458 void ec_master_internal_send_cb(
 
       
   459         void *cb_data /**< Callback data. */
 
       
   460         )
 
       
   461 {
 
       
   462     ec_master_t *master = (ec_master_t *) cb_data;
 
       
   463     down(&master->io_sem);
 
       
   464     ecrt_master_send_ext(master);
 
       
   465     up(&master->io_sem);
 
       
   466 }
 
       
   467 
       
   468 /*****************************************************************************/
 
       
   469 
       
   470 /** Internal receiving callback.
 
       
   471  */
 
       
   472 void ec_master_internal_receive_cb(
 
       
   473         void *cb_data /**< Callback data. */
 
       
   474         )
 
       
   475 {
 
       
   476     ec_master_t *master = (ec_master_t *) cb_data;
 
       
   477     down(&master->io_sem);
 
       
   478     ecrt_master_receive(master);
 
       
   479     up(&master->io_sem);
 
       
   480 }
 
   468 }
 
   481 
   469 
   482 /*****************************************************************************/
 
   470 /*****************************************************************************/
 
   483 
   471 
   484 /** Starts the master thread.
 
   472 /** Starts the master thread.
 
   544 {
 
   532 {
 
   545     int ret;
 
   533     int ret;
 
   546 
   534 
   547     EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n");
 
   535     EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n");
 
   548 
   536 
   549     master->send_cb = ec_master_internal_send_cb;
 
   537     master->fsm_queue_lock_cb = NULL;
 
   550     master->receive_cb = ec_master_internal_receive_cb;
 
   538     master->fsm_queue_unlock_cb = NULL;
 
   551     master->cb_data = master;
 
   539     master->fsm_queue_locking_data = NULL;
 
   552 
   540 
   553     master->phase = EC_IDLE;
 
   541     master->phase = EC_IDLE;
 
   554     ret = ec_master_thread_start(master, ec_master_idle_thread,
 
   542     ret = ec_master_thread_start(master, ec_master_idle_thread,
 
   555             "EtherCAT-IDLE");
 
   543             "EtherCAT-IDLE");
 
   556     if (ret)
 
   544     if (ret)
 
   567 {
 
   555 {
 
   568     EC_MASTER_DBG(master, 1, "IDLE -> ORPHANED.\n");
 
   556     EC_MASTER_DBG(master, 1, "IDLE -> ORPHANED.\n");
 
   569 
   557 
   570     master->phase = EC_ORPHANED;
 
   558     master->phase = EC_ORPHANED;
 
   571     
   559     
   572 #ifdef EC_EOE
 
       
   573     ec_master_eoe_stop(master);
 
       
   574 #endif
 
       
   575     ec_master_thread_stop(master);
 
   560     ec_master_thread_stop(master);
 
   576 
   561 
   577     down(&master->master_sem);
 
   562     ec_mutex_lock(&master->master_mutex);
 
   578     ec_master_clear_slaves(master);
 
   563     ec_master_clear_slaves(master);
 
   579     up(&master->master_sem);
 
   564     ec_mutex_unlock(&master->master_mutex);
 
   580 }
 
   565 }
 
   581 
   566 
   582 /*****************************************************************************/
 
   567 /*****************************************************************************/
 
   583 
   568 
   584 /** Transition function from IDLE to OPERATION phase.
 
   569 /** Transition function from IDLE to OPERATION phase.
 
   591     ec_eoe_t *eoe;
 
   576     ec_eoe_t *eoe;
 
   592 #endif
 
   577 #endif
 
   593 
   578 
   594     EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n");
 
   579     EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n");
 
   595 
   580 
   596     down(&master->config_sem);
 
   581     ec_mutex_lock(&master->config_mutex);
 
   597     master->allow_config = 0; // temporarily disable slave configuration
 
   582     master->allow_config = 0; // temporarily disable slave configuration
 
   598     if (master->config_busy) {
 
   583     if (master->config_busy) {
 
   599         up(&master->config_sem);
 
   584         ec_mutex_unlock(&master->config_mutex);
 
   600 
   585 
   601         // wait for slave configuration to complete
 
   586         // wait for slave configuration to complete
 
   602         ret = wait_event_interruptible(master->config_queue,
 
   587         ret = wait_event_interruptible(master->config_queue,
 
   603                     !master->config_busy);
 
   588                     !master->config_busy);
 
   604         if (ret) {
 
   589         if (ret) {
 
   608         }
 
   593         }
 
   609 
   594 
   610         EC_MASTER_DBG(master, 1, "Waiting for pending slave"
 
   595         EC_MASTER_DBG(master, 1, "Waiting for pending slave"
 
   611                 " configuration returned.\n");
 
   596                 " configuration returned.\n");
 
   612     } else {
 
   597     } else {
 
   613         up(&master->config_sem);
 
   598         ec_mutex_unlock(&master->config_mutex);
 
   614     }
 
   599     }
 
   615 
   600 
   616     down(&master->scan_sem);
 
   601     ec_mutex_lock(&master->scan_mutex);
 
   617     master->allow_scan = 0; // 'lock' the slave list
 
   602     master->allow_scan = 0; // 'lock' the slave list
 
   618     if (!master->scan_busy) {
 
   603     if (!master->scan_busy) {
 
   619         up(&master->scan_sem);
 
   604         ec_mutex_unlock(&master->scan_mutex);
 
   620     } else {
 
   605     } else {
 
   621         up(&master->scan_sem);
 
   606         ec_mutex_unlock(&master->scan_mutex);
 
   622 
   607 
   623         // wait for slave scan to complete
 
   608         // wait for slave scan to complete
 
   624         ret = wait_event_interruptible(master->scan_queue, !master->scan_busy);
 
   609         ret = wait_event_interruptible(master->scan_queue, !master->scan_busy);
 
   625         if (ret) {
 
   610         if (ret) {
 
   626             EC_MASTER_INFO(master, "Waiting for slave scan"
 
   611             EC_MASTER_INFO(master, "Waiting for slave scan"
 
   646             ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
 
   631             ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
 
   647     }
 
   632     }
 
   648 #endif
 
   633 #endif
 
   649 
   634 
   650     master->phase = EC_OPERATION;
 
   635     master->phase = EC_OPERATION;
 
   651     master->app_send_cb = NULL;
 
   636     master->app_fsm_queue_lock_cb = NULL;
 
   652     master->app_receive_cb = NULL;
 
   637     master->app_fsm_queue_unlock_cb = NULL;
 
   653     master->app_cb_data = NULL;
 
   638     master->app_fsm_queue_locking_data = NULL;
 
   654     return ret;
 
   639     return ret;
 
   655     
   640     
   656 out_allow:
 
   641 out_allow:
 
   657     master->allow_scan = 1;
 
   642     master->allow_scan = 1;
 
   658     master->allow_config = 1;
 
   643     master->allow_config = 1;
 
   678     master->phase = EC_IDLE;
 
   663     master->phase = EC_IDLE;
 
   679 }
 
   664 }
 
   680 
   665 
   681 /*****************************************************************************/
 
   666 /*****************************************************************************/
 
   682 
   667 
   683 /** Injects external datagrams that fit into the datagram queue.
 
   668 /** Injects fsm datagrams that fit into the datagram queue.
 
   684  */
 
   669  */
 
   685 void ec_master_inject_external_datagrams(
 
   670 void ec_master_inject_fsm_datagrams(
 
   686         ec_master_t *master /**< EtherCAT master */
 
   671         ec_master_t *master /**< EtherCAT master */
 
   687         )
 
   672         )
 
   688 {
 
   673 {
 
   689     ec_datagram_t *datagram, *n;
 
   674     ec_datagram_t *datagram, *next;
 
   690     size_t queue_size = 0;
 
   675     size_t queue_size = 0;
 
   691 
   676 
       
   677     if (master->fsm_queue_lock_cb)
 
       
   678         master->fsm_queue_lock_cb(master->fsm_queue_locking_data);
 
       
   679     if (ec_mutex_trylock(&master->fsm_queue_mutex) == 0) {
 
       
   680            if (master->fsm_queue_unlock_cb)
 
       
   681                master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
 
       
   682            return;
 
       
   683     }
 
       
   684     if (list_empty(&master->fsm_datagram_queue)) {
 
       
   685         ec_mutex_unlock(&master->fsm_queue_mutex);
 
       
   686         if (master->fsm_queue_unlock_cb)
 
       
   687             master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
 
       
   688         return;
 
       
   689     }
 
   692     list_for_each_entry(datagram, &master->datagram_queue, queue) {
 
   690     list_for_each_entry(datagram, &master->datagram_queue, queue) {
 
   693         queue_size += datagram->data_size;
 
   691         queue_size += datagram->data_size;
 
   694     }
 
   692     }
 
   695 
   693 
   696     list_for_each_entry_safe(datagram, n, &master->external_datagram_queue,
 
   694     list_for_each_entry_safe(datagram, next, &master->fsm_datagram_queue,
 
   697             queue) {
 
   695             fsm_queue) {
 
   698         queue_size += datagram->data_size;
 
   696         queue_size += datagram->data_size;
 
   699         if (queue_size <= master->max_queue_size) {
 
   697         if (queue_size <= master->max_queue_size) {
 
   700             list_del_init(&datagram->queue);
 
   698             list_del_init(&datagram->fsm_queue);
 
   701 #if DEBUG_INJECT
 
   699 #if DEBUG_INJECT
 
   702             EC_MASTER_DBG(master, 0, "Injecting external datagram %08x"
 
   700             EC_MASTER_DBG(master, 2, "Injecting fsm datagram %p"
 
   703                     " size=%u, queue_size=%u\n", (unsigned int) datagram,
 
   701                     " size=%zu, queue_size=%zu\n", datagram,
 
   704                     datagram->data_size, queue_size);
 
   702                     datagram->data_size, queue_size);
 
   705 #endif
 
   703 #endif
 
   706 #ifdef EC_HAVE_CYCLES
 
   704 #ifdef EC_HAVE_CYCLES
 
   707             datagram->cycles_sent = 0;
 
   705             datagram->cycles_sent = 0;
 
   708 #endif
 
   706 #endif
 
   709             datagram->jiffies_sent = 0;
 
   707             datagram->jiffies_sent = 0;
 
   710             ec_master_queue_datagram(master, datagram);
 
   708             ec_master_queue_datagram(master, datagram);
 
   711         }
 
   709         }
 
   712         else {
 
   710         else {
 
   713             if (datagram->data_size > master->max_queue_size) {
 
   711             if (datagram->data_size > master->max_queue_size) {
 
   714                 list_del_init(&datagram->queue);
 
   712                 list_del_init(&datagram->fsm_queue);
 
   715                 datagram->state = EC_DATAGRAM_ERROR;
 
   713                 datagram->state = EC_DATAGRAM_ERROR;
 
   716                 EC_MASTER_ERR(master, "External datagram %p is too large,"
 
   714                 EC_MASTER_ERR(master, "Fsm datagram %p is too large,"
 
   717                         " size=%zu, max_queue_size=%zu\n",
 
   715                         " size=%zu, max_queue_size=%zu\n",
 
   718                         datagram, datagram->data_size,
 
   716                         datagram, datagram->data_size,
 
   719                         master->max_queue_size);
 
   717                         master->max_queue_size);
 
   720             } else {
 
   718             } else {
 
   721 #ifdef EC_HAVE_CYCLES
 
   719 #ifdef EC_HAVE_CYCLES
 
   722                 cycles_t cycles_now = get_cycles();
 
   720                 cycles_t cycles_now = get_cycles();
 
   723 
   721 
   724                 if (cycles_now - datagram->cycles_sent
 
   722                 if (cycles_now - datagram->cycles_sent
 
   725                         > ext_injection_timeout_cycles)
 
   723                         > fsm_injection_timeout_cycles)
 
   726 #else
 
   724 #else
 
   727                 if (jiffies - datagram->jiffies_sent
 
   725                 if (jiffies - datagram->jiffies_sent
 
   728                         > ext_injection_timeout_jiffies)
 
   726                         > fsm_injection_timeout_jiffies)
 
   729 #endif
 
   727 #endif
 
   730                 {
 
   728                 {
 
   731                     unsigned int time_us;
 
   729                     unsigned int time_us;
 
   732 
   730 
   733                     list_del_init(&datagram->queue);
 
   731                     list_del_init(&datagram->fsm_queue);
 
   734                     datagram->state = EC_DATAGRAM_ERROR;
 
   732                     datagram->state = EC_DATAGRAM_ERROR;
 
   735 #ifdef EC_HAVE_CYCLES
 
   733 #ifdef EC_HAVE_CYCLES
 
   736                     time_us = (unsigned int)
 
   734                     time_us = (unsigned int)
 
   737                         ((cycles_now - datagram->cycles_sent) * 1000LL)
 
   735                         ((cycles_now - datagram->cycles_sent) * 1000LL)
 
   738                         / cpu_khz;
 
   736                         / cpu_khz;
 
   739 #else
 
   737 #else
 
   740                     time_us = (unsigned int)
 
   738                     time_us = (unsigned int)
 
   741                         ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
 
   739                         ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
 
   742 #endif
 
   740 #endif
 
   743                     EC_MASTER_ERR(master, "Timeout %u us: Injecting"
 
   741                     EC_MASTER_ERR(master, "Timeout %u us: Injecting"
 
   744                             " external datagram %p size=%zu,"
 
   742                             " fsm datagram %p size=%zu,"
 
   745                             " max_queue_size=%zu\n", time_us, datagram,
 
   743                             " max_queue_size=%zu\n", time_us, datagram,
 
   746                             datagram->data_size, master->max_queue_size);
 
   744                             datagram->data_size, master->max_queue_size);
 
   747                 }
 
   745                 }
 
   748 #if DEBUG_INJECT
 
   746 #if DEBUG_INJECT
 
   749                 else {
 
   747                 else {
 
   750                     EC_MASTER_DBG(master, 0, "Deferred injecting"
 
   748                     EC_MASTER_DBG(master, 2, "Deferred injecting"
 
   751                             " of external datagram %p"
 
   749                             " of fsm datagram %p"
 
   752                             " size=%u, queue_size=%u\n",
 
   750                             " size=%zu, queue_size=%zu\n",
 
   753                             datagram, datagram->data_size, queue_size);
 
   751                             datagram, datagram->data_size, queue_size);
 
   754                 }
 
   752                 }
 
   755 #endif
 
   753 #endif
 
   756             }
 
   754             }
 
   757         }
 
   755         }
 
   758     }
 
   756     }
 
       
   757     ec_mutex_unlock(&master->fsm_queue_mutex);
 
       
   758     if (master->fsm_queue_unlock_cb)
 
       
   759         master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
 
   759 }
 
   760 }
 
   760 
   761 
   761 /*****************************************************************************/
 
   762 /*****************************************************************************/
 
   762 
   763 
   763 /** Sets the expected interval between calls to ecrt_master_send
 
   764 /** Sets the expected interval between calls to ecrt_master_send
 
   774     master->max_queue_size -= master->max_queue_size / 10;
 
   775     master->max_queue_size -= master->max_queue_size / 10;
 
   775 }
 
   776 }
 
   776 
   777 
   777 /*****************************************************************************/
 
   778 /*****************************************************************************/
 
   778 
   779 
   779 /** Places an external datagram in the sdo datagram queue.
 
   780 /** Places an request (SDO/FoE/SoE/EoE) fsm datagram in the sdo datagram queue.
 
   780  */
 
   781  */
 
   781 void ec_master_queue_external_datagram(
 
   782 void ec_master_queue_request_fsm_datagram(
 
   782         ec_master_t *master, /**< EtherCAT master */
 
   783         ec_master_t *master, /**< EtherCAT master */
 
   783         ec_datagram_t *datagram /**< datagram */
 
   784         ec_datagram_t *datagram /**< datagram */
 
   784         )
 
   785         )
 
   785 {
 
   786 {
 
       
   787     ec_master_queue_fsm_datagram(master,datagram);
 
       
   788     master->fsm.idle = 0;   // pump the bus as fast as possible
 
       
   789 }
 
       
   790 
       
   791 /*****************************************************************************/
 
       
   792 
       
   793 /** Places an fsm datagram in the sdo datagram queue.
 
       
   794  */
 
       
   795 void ec_master_queue_fsm_datagram(
 
       
   796         ec_master_t *master, /**< EtherCAT master */
 
       
   797         ec_datagram_t *datagram /**< datagram */
 
       
   798         )
 
       
   799 {
 
   786     ec_datagram_t *queued_datagram;
 
   800     ec_datagram_t *queued_datagram;
 
   787 
   801 
   788     down(&master->io_sem);
 
   802     if (master->fsm_queue_lock_cb)
 
       
   803         master->fsm_queue_lock_cb(master->fsm_queue_locking_data);
 
       
   804     ec_mutex_lock(&master->fsm_queue_mutex);
 
   789 
   805 
   790     // check, if the datagram is already queued
 
   806     // check, if the datagram is already queued
 
   791     list_for_each_entry(queued_datagram, &master->external_datagram_queue,
 
   807     list_for_each_entry(queued_datagram, &master->fsm_datagram_queue,
 
   792             queue) {
 
   808             fsm_queue) {
 
   793         if (queued_datagram == datagram) {
 
   809         if (queued_datagram == datagram) {
 
   794             datagram->state = EC_DATAGRAM_QUEUED;
 
   810             datagram->state = EC_DATAGRAM_QUEUED;
 
       
   811             ec_mutex_unlock(&master->fsm_queue_mutex);
 
       
   812             if (master->fsm_queue_unlock_cb)
 
       
   813                 master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
 
   795             return;
 
   814             return;
 
   796         }
 
   815         }
 
   797     }
 
   816     }
 
   798 
   817 
   799 #if DEBUG_INJECT
 
   818 #if DEBUG_INJECT
 
   800     EC_MASTER_DBG(master, 0, "Requesting external datagram %p size=%u\n",
 
   819     EC_MASTER_DBG(master, 2, "Requesting fsm datagram %p size=%zu\n",
 
   801             datagram, datagram->data_size);
 
   820             datagram, datagram->data_size);
 
   802 #endif
 
   821 #endif
 
   803 
   822 
   804     list_add_tail(&datagram->queue, &master->external_datagram_queue);
 
   823     list_add_tail(&datagram->fsm_queue, &master->fsm_datagram_queue);
 
   805     datagram->state = EC_DATAGRAM_QUEUED;
 
   824     datagram->state = EC_DATAGRAM_QUEUED;
 
   806 #ifdef EC_HAVE_CYCLES
 
   825 #ifdef EC_HAVE_CYCLES
 
   807     datagram->cycles_sent = get_cycles();
 
   826     datagram->cycles_sent = get_cycles();
 
   808 #endif
 
   827 #endif
 
   809     datagram->jiffies_sent = jiffies;
 
   828     datagram->jiffies_sent = jiffies;
 
   810 
   829 
   811     master->fsm.idle = 0;
 
   830     ec_mutex_unlock(&master->fsm_queue_mutex);
 
   812     up(&master->io_sem);
 
   831     if (master->fsm_queue_unlock_cb)
 
       
   832         master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
 
   813 }
 
   833 }
 
   814 
   834 
   815 /*****************************************************************************/
 
   835 /*****************************************************************************/
 
   816 
   836 
   817 /** Places a datagram in the datagram queue.
 
   837 /** Places a datagram in the datagram queue.
 
   837 
   857 
   838     list_add_tail(&datagram->queue, &master->datagram_queue);
 
   858     list_add_tail(&datagram->queue, &master->datagram_queue);
 
   839     datagram->state = EC_DATAGRAM_QUEUED;
 
   859     datagram->state = EC_DATAGRAM_QUEUED;
 
   840 }
 
   860 }
 
   841 
   861 
   842 /*****************************************************************************/
 
       
   843 
       
   844 /** Places a datagram in the non-application datagram queue.
 
       
   845  */
 
       
   846 void ec_master_queue_datagram_ext(
 
       
   847         ec_master_t *master, /**< EtherCAT master */
 
       
   848         ec_datagram_t *datagram /**< datagram */
 
       
   849         )
 
       
   850 {
 
       
   851     down(&master->ext_queue_sem);
 
       
   852     list_add_tail(&datagram->queue, &master->ext_datagram_queue);
 
       
   853     up(&master->ext_queue_sem);
 
       
   854 }
 
       
   855 
   862 
   856 /*****************************************************************************/
 
   863 /*****************************************************************************/
 
   857 
   864 
   858 /** Sends the datagrams in the queue.
 
   865 /** Sends the datagrams in the queue.
 
   859  *
 
   866  *
 
   860  */
 
   867  */
 
   861 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
 
   868 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
 
   862 {
 
   869 {
 
   863     ec_datagram_t *datagram, *next;
 
   870     ec_datagram_t *datagram, *next;
 
   864     size_t datagram_size;
 
   871     size_t datagram_size;
 
   865     uint8_t *frame_data, *cur_data;
 
   872     uint8_t *frame_data, *cur_data, *frame_datagram_data;
 
   866     void *follows_word;
 
   873     void *follows_word;
 
   867 #ifdef EC_HAVE_CYCLES
 
   874 #ifdef EC_HAVE_CYCLES
 
   868     cycles_t cycles_start, cycles_sent, cycles_end;
 
   875     cycles_t cycles_start, cycles_sent, cycles_end;
 
   869 #endif
 
   876 #endif
 
   870     unsigned long jiffies_sent;
 
   877     unsigned long jiffies_sent;
 
   871     unsigned int frame_count, more_datagrams_waiting;
 
   878     unsigned int frame_count, more_datagrams_waiting;
 
   872     struct list_head sent_datagrams;
 
   879     struct list_head sent_datagrams;
 
       
   880     ec_fmmu_config_t* domain_fmmu;
 
   873 
   881 
   874 #ifdef EC_HAVE_CYCLES
 
   882 #ifdef EC_HAVE_CYCLES
 
   875     cycles_start = get_cycles();
 
   883     cycles_start = get_cycles();
 
   876 #endif
 
   884 #endif
 
   877     frame_count = 0;
 
   885     frame_count = 0;
 
   899             }
 
   907             }
 
   900 
   908 
   901             list_add_tail(&datagram->sent, &sent_datagrams);
 
   909             list_add_tail(&datagram->sent, &sent_datagrams);
 
   902             datagram->index = master->datagram_index++;
 
   910             datagram->index = master->datagram_index++;
 
   903 
   911 
   904             EC_MASTER_DBG(master, 2, "adding datagram 0x%02X\n",
 
   912             EC_MASTER_DBG(master, 2, "adding datagram %p i=0x%02X size=%zu\n",datagram,
 
   905                     datagram->index);
 
   913                     datagram->index,datagram_size);
 
   906 
   914 
   907             // set "datagram following" flag in previous frame
 
   915             // set "datagram following" flag in previous frame
 
   908             if (follows_word)
 
   916             if (follows_word)
 
   909                 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);
 
   917                 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);
 
   910 
   918 
   916             EC_WRITE_U16(cur_data + 8, 0x0000);
 
   924             EC_WRITE_U16(cur_data + 8, 0x0000);
 
   917             follows_word = cur_data + 6;
 
   925             follows_word = cur_data + 6;
 
   918             cur_data += EC_DATAGRAM_HEADER_SIZE;
 
   926             cur_data += EC_DATAGRAM_HEADER_SIZE;
 
   919 
   927 
   920             // EtherCAT datagram data
 
   928             // EtherCAT datagram data
 
   921             memcpy(cur_data, datagram->data, datagram->data_size);
 
   929             frame_datagram_data = cur_data;
 
       
   930             if (datagram->domain) {
 
       
   931                 unsigned int datagram_address = EC_READ_U32(datagram->address);
 
       
   932                 int i = 0;
 
       
   933                 uint8_t *domain_data = datagram->data;
 
       
   934                 list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) {
 
       
   935                     if (domain_fmmu->dir == EC_DIR_OUTPUT ) {
 
       
   936                         unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address;
 
       
   937                         memcpy(frame_datagram_data+frame_offset, domain_data, domain_fmmu->data_size);
 
       
   938                         if (unlikely(master->debug_level > 1)) {
 
       
   939                             EC_DBG("sending dg %p i=0x%02X fmmu %u fp=%u dp=%zu size=%u\n",
 
       
   940                                    datagram,datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size);
 
       
   941                             ec_print_data(domain_data, domain_fmmu->data_size);
 
       
   942                         }
 
       
   943                     }
 
       
   944                     domain_data += domain_fmmu->data_size;
 
       
   945                     ++i;
 
       
   946                 }
 
       
   947             }
 
       
   948             else {
 
       
   949                 memcpy(frame_datagram_data, datagram->data, datagram->data_size);
 
       
   950             }
 
   922             cur_data += datagram->data_size;
 
   951             cur_data += datagram->data_size;
 
   923 
   952 
   924             // EtherCAT datagram footer
 
   953             // EtherCAT datagram footer
 
   925             EC_WRITE_U16(cur_data, 0x0000); // reset working counter
 
   954             EC_WRITE_U16(cur_data, 0x0000); // reset working counter
 
   926             cur_data += EC_DATAGRAM_FOOTER_SIZE;
 
   955             cur_data += EC_DATAGRAM_FOOTER_SIZE;
 
   986                                  )
 
  1015                                  )
 
   987 {
 
  1016 {
 
   988     size_t frame_size, data_size;
 
  1017     size_t frame_size, data_size;
 
   989     uint8_t datagram_type, datagram_index;
 
  1018     uint8_t datagram_type, datagram_index;
 
   990     unsigned int cmd_follows, matched;
 
  1019     unsigned int cmd_follows, matched;
 
   991     const uint8_t *cur_data;
 
  1020     const uint8_t *cur_data, *frame_datagram_data;
 
   992     ec_datagram_t *datagram;
 
  1021     ec_datagram_t *datagram;
 
       
  1022     ec_fmmu_config_t* domain_fmmu;
 
   993 
  1023 
   994     if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
 
  1024     if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
 
   995         if (master->debug_level) {
 
  1025         if (master->debug_level) {
 
   996             EC_MASTER_DBG(master, 0, "Corrupted frame received"
 
  1026             EC_MASTER_DBG(master, 0, "Corrupted frame received"
 
   997                     " (size %zu < %u byte):\n",
 
  1027                     " (size %zu < %u byte):\n",
 
  1070             }
 
  1100             }
 
  1071 
  1101 
  1072             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
 
  1102             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
 
  1073             continue;
 
  1103             continue;
 
  1074         }
 
  1104         }
 
  1075 
  1105         frame_datagram_data = cur_data;
 
  1076         // copy received data into the datagram memory
 
  1106         if (datagram->domain) {
 
  1077         memcpy(datagram->data, cur_data, data_size);
 
  1107             size_t datagram_address = EC_READ_U32(datagram->address);
 
       
  1108             int i = 0;
 
       
  1109             uint8_t *domain_data = datagram->data;
 
       
  1110             list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) {
 
       
  1111                 if (domain_fmmu->dir == EC_DIR_INPUT ) {
 
       
  1112                     unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address;
 
       
  1113                     memcpy(domain_data, frame_datagram_data+frame_offset, domain_fmmu->data_size);
 
       
  1114                     if (unlikely(master->debug_level > 1)) {
 
       
  1115                         EC_DBG("receiving dg %p i=0x%02X fmmu %u fp=%u dp=%zu size=%u\n",
 
       
  1116                                datagram,datagram->index, i,
 
       
  1117                                frame_offset,domain_data-datagram->data,domain_fmmu->data_size);
 
       
  1118                         ec_print_data(domain_data, domain_fmmu->data_size);
 
       
  1119                     }
 
       
  1120                 }
 
       
  1121                 domain_data += domain_fmmu->data_size;
 
       
  1122                 ++i;
 
       
  1123             }
 
       
  1124         }
 
       
  1125         else {
 
       
  1126             // copy received data into the datagram memory
 
       
  1127             memcpy(datagram->data, frame_datagram_data, data_size);
 
       
  1128         }
 
  1078         cur_data += data_size;
 
  1129         cur_data += data_size;
 
  1079 
  1130 
  1080         // set the datagram's working counter
 
  1131         // set the datagram's working counter
 
  1081         datagram->working_counter = EC_READ_U16(cur_data);
 
  1132         datagram->working_counter = EC_READ_U16(cur_data);
 
  1082         cur_data += EC_DATAGRAM_FOOTER_SIZE;
 
  1133         cur_data += EC_DATAGRAM_FOOTER_SIZE;
 
  1085         datagram->state = EC_DATAGRAM_RECEIVED;
 
  1136         datagram->state = EC_DATAGRAM_RECEIVED;
 
  1086 #ifdef EC_HAVE_CYCLES
 
  1137 #ifdef EC_HAVE_CYCLES
 
  1087         datagram->cycles_received = master->main_device.cycles_poll;
 
  1138         datagram->cycles_received = master->main_device.cycles_poll;
 
  1088 #endif
 
  1139 #endif
 
  1089         datagram->jiffies_received = master->main_device.jiffies_poll;
 
  1140         datagram->jiffies_received = master->main_device.jiffies_poll;
 
       
  1141         EC_MASTER_DBG(master, 2, "removing datagram %p i=0x%02X\n",datagram,
 
       
  1142                 datagram->index);
 
  1090         list_del_init(&datagram->queue);
 
  1143         list_del_init(&datagram->queue);
 
  1091     }
 
  1144     }
 
  1092 }
 
  1145 }
 
  1093 
  1146 
  1094 /*****************************************************************************/
 
  1147 /*****************************************************************************/
 
  1206  */
 
  1259  */
 
  1207 static int ec_master_idle_thread(void *priv_data)
 
  1260 static int ec_master_idle_thread(void *priv_data)
 
  1208 {
 
  1261 {
 
  1209     ec_master_t *master = (ec_master_t *) priv_data;
 
  1262     ec_master_t *master = (ec_master_t *) priv_data;
 
  1210     ec_slave_t *slave = NULL;
 
  1263     ec_slave_t *slave = NULL;
 
  1211     int fsm_exec;
 
       
  1212     size_t sent_bytes;
 
  1264     size_t sent_bytes;
 
  1213 
       
  1214     // send interval in IDLE phase
 
  1265     // send interval in IDLE phase
 
  1215     ec_master_set_send_interval(master, 1000000 / HZ); 
  1266     ec_master_set_send_interval(master, 1000000 / HZ); 
  1216 
  1267 
  1217     EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us,"
 
  1268     EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us,"
 
  1218             " max data size=%zu\n", master->send_interval,
 
  1269             " max data size=%zu\n", master->send_interval,
 
  1220 
  1271 
  1221     while (!kthread_should_stop()) {
 
  1272     while (!kthread_should_stop()) {
 
  1222         ec_datagram_output_stats(&master->fsm_datagram);
 
  1273         ec_datagram_output_stats(&master->fsm_datagram);
 
  1223 
  1274 
  1224         // receive
 
  1275         // receive
 
  1225         down(&master->io_sem);
 
  1276         ec_mutex_lock(&master->io_mutex);
 
  1226         ecrt_master_receive(master);
 
  1277         ecrt_master_receive(master);
 
  1227         up(&master->io_sem);
 
  1278         ec_mutex_unlock(&master->io_mutex);
 
  1228 
  1279 
  1229         fsm_exec = 0;
 
       
  1230         // execute master & slave state machines
 
  1280         // execute master & slave state machines
 
  1231         if (down_interruptible(&master->master_sem))
 
  1281         if (ec_mutex_lock_interruptible(&master->master_mutex))
 
  1232             break;
 
  1282             break;
 
  1233         fsm_exec = ec_fsm_master_exec(&master->fsm);
 
  1283         if (ec_fsm_master_exec(&master->fsm)) {
 
       
  1284             ec_master_mbox_queue_datagrams(master, &master->fsm_mbox);
 
       
  1285         }
 
  1234         for (slave = master->slaves;
 
  1286         for (slave = master->slaves;
 
  1235                 slave < master->slaves + master->slave_count;
 
  1287                 slave < master->slaves + master->slave_count;
 
  1236                 slave++) {
 
  1288                 slave++) {
 
  1237             ec_fsm_slave_exec(&slave->fsm);
 
  1289             ec_fsm_slave_exec(&slave->fsm); // may queue datagram in fsm queue
 
  1238         }
 
  1290         }
 
  1239         up(&master->master_sem);
 
  1291 #if defined(EC_EOE)
 
       
  1292         if (!ec_master_eoe_processing(master))
 
       
  1293             master->fsm.idle = 0;  // pump the bus as fast as possible
 
       
  1294 #endif
 
       
  1295         ec_mutex_unlock(&master->master_mutex);
 
  1240 
  1296 
  1241         // queue and send
 
  1297         // queue and send
 
  1242         down(&master->io_sem);
 
  1298         ec_mutex_lock(&master->io_mutex);
 
  1243         if (fsm_exec) {
 
       
  1244             ec_master_queue_datagram(master, &master->fsm_datagram);
 
       
  1245         }
 
       
  1246         ec_master_inject_external_datagrams(master);
 
       
  1247         ecrt_master_send(master);
 
  1299         ecrt_master_send(master);
 
  1248         sent_bytes = master->main_device.tx_skb[
 
  1300         sent_bytes = master->main_device.tx_skb[
 
  1249             master->main_device.tx_ring_index]->len;
 
  1301             master->main_device.tx_ring_index]->len;
 
  1250         up(&master->io_sem);
 
  1302         ec_mutex_unlock(&master->io_mutex);
 
  1251 
  1303 
  1252         if (ec_fsm_master_idle(&master->fsm)) {
 
  1304         if (ec_fsm_master_idle(&master->fsm)) {
 
  1253 #ifdef EC_USE_HRTIMER
 
  1305 #ifdef EC_USE_HRTIMER
 
  1254             ec_master_nanosleep(master->send_interval * 1000);
 
  1306             ec_master_nanosleep(master->send_interval * 1000);
 
  1255 #else
 
  1307 #else
 
  1276  */
 
  1328  */
 
  1277 static int ec_master_operation_thread(void *priv_data)
 
  1329 static int ec_master_operation_thread(void *priv_data)
 
  1278 {
 
  1330 {
 
  1279     ec_master_t *master = (ec_master_t *) priv_data;
 
  1331     ec_master_t *master = (ec_master_t *) priv_data;
 
  1280     ec_slave_t *slave = NULL;
 
  1332     ec_slave_t *slave = NULL;
 
  1281     int fsm_exec;
 
       
  1282 
  1333 
  1283     EC_MASTER_DBG(master, 1, "Operation thread running"
 
  1334     EC_MASTER_DBG(master, 1, "Operation thread running"
 
  1284             " with fsm interval = %u us, max data size=%zu\n",
 
  1335             " with fsm interval = %u us, max data size=%zu\n",
 
  1285             master->send_interval, master->max_queue_size);
 
  1336             master->send_interval, master->max_queue_size);
 
  1286 
  1337 
  1287     while (!kthread_should_stop()) {
 
  1338     while (!kthread_should_stop()) {
 
  1288         ec_datagram_output_stats(&master->fsm_datagram);
 
  1339         ec_datagram_output_stats(&master->fsm_datagram);
 
  1289 
  1340 
  1290         if (master->injection_seq_rt == master->injection_seq_fsm) {
 
  1341         // output statistics
 
  1291             // output statistics
 
  1342         ec_master_output_stats(master);
 
  1292             ec_master_output_stats(master);
 
  1343 
  1293 
  1344         // execute master & slave state machines
 
  1294             fsm_exec = 0;
 
  1345         if (ec_mutex_lock_interruptible(&master->master_mutex))
 
  1295             // execute master & slave state machines
 
  1346             break;
 
  1296             if (down_interruptible(&master->master_sem))
 
  1347         if (ec_fsm_master_exec(&master->fsm))
 
  1297                 break;
 
  1348             ec_master_mbox_queue_datagrams(master, &master->fsm_mbox);
 
  1298             fsm_exec += ec_fsm_master_exec(&master->fsm);
 
  1349         for (slave = master->slaves;
 
  1299             for (slave = master->slaves;
 
  1350                 slave < master->slaves + master->slave_count;
 
  1300                     slave < master->slaves + master->slave_count;
 
  1351                 slave++) {
 
  1301                     slave++) {
 
  1352             ec_fsm_slave_exec(&slave->fsm); // may queue datagram in fsm queue
 
  1302                 ec_fsm_slave_exec(&slave->fsm);
 
  1353         }
 
  1303             }
 
  1354 #if defined(EC_EOE)
 
  1304             up(&master->master_sem);
 
  1355         ec_master_eoe_processing(master);
 
  1305 
  1356 #endif
 
  1306             // inject datagrams (let the rt thread queue them, see ecrt_master_send)
 
  1357         ec_mutex_unlock(&master->master_mutex);
 
  1307             if (fsm_exec)
 
       
  1308                 master->injection_seq_fsm++;
 
       
  1309         }
 
       
  1310 
  1358 
  1311 #ifdef EC_USE_HRTIMER
 
  1359 #ifdef EC_USE_HRTIMER
 
  1312         // the op thread should not work faster than the sending RT thread
 
  1360         // the op thread should not work faster than the sending RT thread
 
  1313         ec_master_nanosleep(master->send_interval * 1000);
 
  1361         ec_master_nanosleep(master->send_interval * 1000);
 
  1314 #else
 
  1362 #else
 
  1327 }
 
  1375 }
 
  1328 
  1376 
  1329 /*****************************************************************************/
 
  1377 /*****************************************************************************/
 
  1330 
  1378 
  1331 #ifdef EC_EOE
 
  1379 #ifdef EC_EOE
 
  1332 /** Starts Ethernet over EtherCAT processing on demand.
 
       
  1333  */
 
       
  1334 void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */)
 
       
  1335 {
 
       
  1336     struct sched_param param = { .sched_priority = 0 };
 
       
  1337 
       
  1338     if (master->eoe_thread) {
 
       
  1339         EC_MASTER_WARN(master, "EoE already running!\n");
 
       
  1340         return;
 
       
  1341     }
 
       
  1342 
       
  1343     if (list_empty(&master->eoe_handlers))
 
       
  1344         return;
 
       
  1345 
       
  1346     if (!master->send_cb || !master->receive_cb) {
 
       
  1347         EC_MASTER_WARN(master, "No EoE processing"
 
       
  1348                 " because of missing callbacks!\n");
 
       
  1349         return;
 
       
  1350     }
 
       
  1351 
       
  1352     EC_MASTER_INFO(master, "Starting EoE thread.\n");
 
       
  1353     master->eoe_thread = kthread_run(ec_master_eoe_thread, master,
 
       
  1354             "EtherCAT-EoE");
 
       
  1355     if (IS_ERR(master->eoe_thread)) {
 
       
  1356         int err = (int) PTR_ERR(master->eoe_thread);
 
       
  1357         EC_MASTER_ERR(master, "Failed to start EoE thread (error %i)!\n",
 
       
  1358                 err);
 
       
  1359         master->eoe_thread = NULL;
 
       
  1360         return;
 
       
  1361     }
 
       
  1362 
       
  1363     sched_setscheduler(master->eoe_thread, SCHED_NORMAL, &param);
 
       
  1364     set_user_nice(master->eoe_thread, 0);
 
       
  1365 }
 
       
  1366 
       
  1367 /*****************************************************************************/
 
       
  1368 
       
  1369 /** Stops the Ethernet over EtherCAT processing.
 
       
  1370  */
 
       
  1371 void ec_master_eoe_stop(ec_master_t *master /**< EtherCAT master */)
 
       
  1372 {
 
       
  1373     if (master->eoe_thread) {
 
       
  1374         EC_MASTER_INFO(master, "Stopping EoE thread.\n");
 
       
  1375 
       
  1376         kthread_stop(master->eoe_thread);
 
       
  1377         master->eoe_thread = NULL;
 
       
  1378         EC_MASTER_INFO(master, "EoE thread exited.\n");
 
       
  1379     }
 
       
  1380 }
 
       
  1381 
  1380 
  1382 /*****************************************************************************/
 
  1381 /*****************************************************************************/
 
  1383 
  1382 
  1384 /** Does the Ethernet over EtherCAT processing.
 
  1383 /** Does the Ethernet over EtherCAT processing.
 
  1385  */
 
  1384  */
 
  1386 static int ec_master_eoe_thread(void *priv_data)
 
  1385 static int ec_master_eoe_processing(ec_master_t *master)
 
  1387 {
 
  1386 {
 
  1388     ec_master_t *master = (ec_master_t *) priv_data;
 
       
  1389     ec_eoe_t *eoe;
 
  1387     ec_eoe_t *eoe;
 
  1390     unsigned int none_open, sth_to_send, all_idle;
 
  1388     unsigned int none_open, sth_to_send, all_idle;
 
  1391 
  1389     none_open = 1;
 
  1392     EC_MASTER_DBG(master, 1, "EoE thread running.\n");
 
  1390     all_idle = 1;
 
  1393 
  1391 
  1394     while (!kthread_should_stop()) {
 
  1392     list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
  1395         none_open = 1;
 
  1393         if (ec_eoe_is_open(eoe)) {
 
  1396         all_idle = 1;
 
  1394             none_open = 0;
 
  1397 
  1395             break;
 
       
  1396         }
 
       
  1397     }
 
       
  1398     if (none_open)
 
       
  1399         return all_idle;
 
       
  1400 
       
  1401     // actual EoE processing
 
       
  1402     sth_to_send = 0;
 
       
  1403     list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
       
  1404         ec_eoe_run(eoe);
 
       
  1405         if (eoe->queue_datagram) {
 
       
  1406             sth_to_send = 1;
 
       
  1407         }
 
       
  1408         if (!ec_eoe_is_idle(eoe)) {
 
       
  1409             all_idle = 0;
 
       
  1410         }
 
       
  1411     }
 
       
  1412 
       
  1413     if (sth_to_send) {
 
  1398         list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
  1414         list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
  1399             if (ec_eoe_is_open(eoe)) {
 
  1415             ec_eoe_queue(eoe);
 
  1400                 none_open = 0;
 
  1416         }
 
  1401                 break;
 
  1417     }
 
  1402             }
 
  1418     return all_idle;
 
  1403         }
 
  1419 }
 
  1404         if (none_open)
 
  1420 
  1405             goto schedule;
 
  1421 #endif // EC_EOE
 
  1406 
       
  1407         // receive datagrams
 
       
  1408         master->receive_cb(master->cb_data);
 
       
  1409 
       
  1410         // actual EoE processing
 
       
  1411         sth_to_send = 0;
 
       
  1412         list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
       
  1413             ec_eoe_run(eoe);
 
       
  1414             if (eoe->queue_datagram) {
 
       
  1415                 sth_to_send = 1;
 
       
  1416             }
 
       
  1417             if (!ec_eoe_is_idle(eoe)) {
 
       
  1418                 all_idle = 0;
 
       
  1419             }
 
       
  1420         }
 
       
  1421 
       
  1422         if (sth_to_send) {
 
       
  1423             list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
       
  1424                 ec_eoe_queue(eoe);
 
       
  1425             }
 
       
  1426             // (try to) send datagrams
 
       
  1427             down(&master->ext_queue_sem);
 
       
  1428             master->send_cb(master->cb_data);
 
       
  1429             up(&master->ext_queue_sem);
 
       
  1430         }
 
       
  1431 
       
  1432 schedule:
 
       
  1433         if (all_idle) {
 
       
  1434             set_current_state(TASK_INTERRUPTIBLE);
 
       
  1435             schedule_timeout(1);
 
       
  1436         } else {
 
       
  1437             schedule();
 
       
  1438         }
 
       
  1439     }
 
       
  1440     
       
  1441     EC_MASTER_DBG(master, 1, "EoE thread exiting...\n");
 
       
  1442     return 0;
 
       
  1443 }
 
       
  1444 #endif
 
       
  1445 
  1422 
  1446 /*****************************************************************************/
 
  1423 /*****************************************************************************/
 
  1447 
  1424 
  1448 /** Detaches the slave configurations from the slaves.
 
  1425 /** Detaches the slave configurations from the slaves.
 
  1449  */
 
  1426  */
 
  1779     unsigned int i;
 
  1756     unsigned int i;
 
  1780     int ret;
 
  1757     int ret;
 
  1781 
  1758 
  1782     slave->ports[0].next_slave = port0_slave;
 
  1759     slave->ports[0].next_slave = port0_slave;
 
  1783 
  1760 
  1784     for (i = 1; i < EC_MAX_PORTS; i++) {
 
  1761     i = 3;
 
       
  1762     while (i != 0) {
 
  1785         if (!slave->ports[i].link.loop_closed) {
 
  1763         if (!slave->ports[i].link.loop_closed) {
 
  1786             *slave_position = *slave_position + 1;
 
  1764             *slave_position = *slave_position + 1;
 
  1787             if (*slave_position < master->slave_count) {
 
  1765             if (*slave_position < master->slave_count) {
 
  1788                 slave->ports[i].next_slave = master->slaves + *slave_position;
 
  1766                 slave->ports[i].next_slave = master->slaves + *slave_position;
 
  1789                 ret = ec_master_calc_topology_rec(master,
 
  1767                 ret = ec_master_calc_topology_rec(master,
 
  1792                     return ret;
 
  1770                     return ret;
 
  1793             } else {
 
  1771             } else {
 
  1794                 return -1;
 
  1772                 return -1;
 
  1795             }
 
  1773             }
 
  1796         }
 
  1774         }
 
       
  1775         switch (i)
 
       
  1776         {
 
       
  1777         case 0: i = 3; break;
 
       
  1778         case 1: i = 2; break;
 
       
  1779         case 3: i = 1; break;
 
       
  1780         case 2:
 
       
  1781         default:i = 0; break;
 
       
  1782         }
 
  1797     }
 
  1783     }
 
  1798 
  1784 
  1799     return 0;
 
  1785     return 0;
 
  1800 }
 
  1786 }
 
  1801 
  1787 
  1905     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
  1891     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
  1906         EC_MASTER_ERR(master, "Error allocating domain memory!\n");
 
  1892         EC_MASTER_ERR(master, "Error allocating domain memory!\n");
 
  1907         return ERR_PTR(-ENOMEM);
 
  1893         return ERR_PTR(-ENOMEM);
 
  1908     }
 
  1894     }
 
  1909 
  1895 
  1910     down(&master->master_sem);
 
  1896     ec_mutex_lock(&master->master_mutex);
 
  1911 
  1897 
  1912     if (list_empty(&master->domains)) {
 
  1898     if (list_empty(&master->domains)) {
 
  1913         index = 0;
 
  1899         index = 0;
 
  1914     } else {
 
  1900     } else {
 
  1915         last_domain = list_entry(master->domains.prev, ec_domain_t, list);
 
  1901         last_domain = list_entry(master->domains.prev, ec_domain_t, list);
 
  1917     }
 
  1903     }
 
  1918 
  1904 
  1919     ec_domain_init(domain, master, index);
 
  1905     ec_domain_init(domain, master, index);
 
  1920     list_add_tail(&domain->list, &master->domains);
 
  1906     list_add_tail(&domain->list, &master->domains);
 
  1921 
  1907 
  1922     up(&master->master_sem);
 
  1908     ec_mutex_unlock(&master->master_mutex);
 
  1923 
  1909 
  1924     EC_MASTER_DBG(master, 1, "Created domain %u.\n", domain->index);
 
  1910     EC_MASTER_DBG(master, 1, "Created domain %u.\n", domain->index);
 
  1925 
  1911 
  1926     return domain;
 
  1912     return domain;
 
  1927 }
 
  1913 }
 
  1941 int ecrt_master_activate(ec_master_t *master)
 
  1927 int ecrt_master_activate(ec_master_t *master)
 
  1942 {
 
  1928 {
 
  1943     uint32_t domain_offset;
 
  1929     uint32_t domain_offset;
 
  1944     ec_domain_t *domain;
 
  1930     ec_domain_t *domain;
 
  1945     int ret;
 
  1931     int ret;
 
  1946 #ifdef EC_EOE
 
       
  1947     int eoe_was_running;
 
       
  1948 #endif
 
       
  1949 
  1932 
  1950     EC_MASTER_DBG(master, 1, "ecrt_master_activate(master = 0x%p)\n", master);
 
  1933     EC_MASTER_DBG(master, 1, "ecrt_master_activate(master = 0x%p)\n", master);
 
  1951 
  1934 
  1952     if (master->active) {
 
  1935     if (master->active) {
 
  1953         EC_MASTER_WARN(master, "%s: Master already active!\n", __func__);
 
  1936         EC_MASTER_WARN(master, "%s: Master already active!\n", __func__);
 
  1954         return 0;
 
  1937         return 0;
 
  1955     }
 
  1938     }
 
  1956 
  1939 
  1957     down(&master->master_sem);
 
  1940     ec_mutex_lock(&master->master_mutex);
 
  1958 
  1941 
  1959     // finish all domains
 
  1942     // finish all domains
 
  1960     domain_offset = 0;
 
  1943     domain_offset = 0;
 
  1961     list_for_each_entry(domain, &master->domains, list) {
 
  1944     list_for_each_entry(domain, &master->domains, list) {
 
  1962         ret = ec_domain_finish(domain, domain_offset);
 
  1945         ret = ec_domain_finish(domain, domain_offset);
 
  1963         if (ret < 0) {
 
  1946         if (ret < 0) {
 
  1964             up(&master->master_sem);
 
  1947             ec_mutex_unlock(&master->master_mutex);
 
  1965             EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain);
 
  1948             EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain);
 
  1966             return ret;
 
  1949             return ret;
 
  1967         }
 
  1950         }
 
  1968         domain_offset += domain->data_size;
 
  1951         domain_offset += domain->data_size;
 
  1969     }
 
  1952     }
 
  1970     
  1953     
  1971     up(&master->master_sem);
 
  1954     ec_mutex_unlock(&master->master_mutex);
 
  1972 
  1955 
  1973     // restart EoE process and master thread with new locking
 
  1956     // restart EoE process and master thread with new locking
 
  1974 
  1957 
  1975     ec_master_thread_stop(master);
 
  1958     ec_master_thread_stop(master);
 
  1976 #ifdef EC_EOE
 
       
  1977     eoe_was_running = master->eoe_thread != NULL;
 
       
  1978     ec_master_eoe_stop(master);
 
       
  1979 #endif
 
       
  1980 
  1959 
  1981     EC_MASTER_DBG(master, 1, "FSM datagram is %p.\n", &master->fsm_datagram);
 
  1960     EC_MASTER_DBG(master, 1, "FSM datagram is %p.\n", &master->fsm_datagram);
 
  1982 
  1961 
  1983     master->injection_seq_fsm = 0;
 
  1962     master->injection_seq_fsm = 0;
 
  1984     master->injection_seq_rt = 0;
 
  1963     master->injection_seq_rt = 0;
 
  1985 
  1964 
  1986     master->send_cb = master->app_send_cb;
 
  1965     master->fsm_queue_lock_cb = master->app_fsm_queue_lock_cb;
 
  1987     master->receive_cb = master->app_receive_cb;
 
  1966     master->fsm_queue_unlock_cb = master->app_fsm_queue_unlock_cb;
 
  1988     master->cb_data = master->app_cb_data;
 
  1967     master->fsm_queue_locking_data = master->app_fsm_queue_locking_data;
 
  1989     
  1968     
  1990 #ifdef EC_EOE
 
       
  1991     if (eoe_was_running) {
 
       
  1992         ec_master_eoe_start(master);
 
       
  1993     }
 
       
  1994 #endif
 
       
  1995     ret = ec_master_thread_start(master, ec_master_operation_thread,
 
  1969     ret = ec_master_thread_start(master, ec_master_operation_thread,
 
  1996                 "EtherCAT-OP");
 
  1970                 "EtherCAT-OP");
 
  1997     if (ret < 0) {
 
  1971     if (ret < 0) {
 
  1998         EC_MASTER_ERR(master, "Failed to start master thread!\n");
 
  1972         EC_MASTER_ERR(master, "Failed to start master thread!\n");
 
  1999         return ret;
 
  1973         return ret;
 
  2014 void ecrt_master_deactivate(ec_master_t *master)
 
  1988 void ecrt_master_deactivate(ec_master_t *master)
 
  2015 {
 
  1989 {
 
  2016     ec_slave_t *slave;
 
  1990     ec_slave_t *slave;
 
  2017 #ifdef EC_EOE
 
  1991 #ifdef EC_EOE
 
  2018     ec_eoe_t *eoe;
 
  1992     ec_eoe_t *eoe;
 
  2019     int eoe_was_running;
 
  1993     int is_eoe_slave;
 
  2020 #endif
 
  1994 #endif
 
  2021 
  1995 
  2022     EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master);
 
  1996     EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master);
 
  2023 
  1997 
  2024     if (!master->active) {
 
  1998     if (!master->active) {
 
  2025         EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
 
  1999         EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
 
  2026         return;
 
  2000         return;
 
  2027     }
 
  2001     }
 
  2028 
  2002 
  2029     ec_master_thread_stop(master);
 
  2003     ec_master_thread_stop(master);
 
  2030 #ifdef EC_EOE
 
       
  2031     eoe_was_running = master->eoe_thread != NULL;
 
       
  2032     ec_master_eoe_stop(master);
 
       
  2033 #endif
 
       
  2034     
  2004     
  2035     master->send_cb = ec_master_internal_send_cb;
 
  2005     master->fsm_queue_lock_cb = NULL;
 
  2036     master->receive_cb = ec_master_internal_receive_cb;
 
  2006     master->fsm_queue_unlock_cb= NULL;
 
  2037     master->cb_data = master;
 
  2007     master->fsm_queue_locking_data = NULL;
 
  2038     
  2008     
  2039     ec_master_clear_config(master);
 
  2009     ec_master_clear_config(master);
 
  2040 
  2010 
  2041     for (slave = master->slaves;
 
  2011     for (slave = master->slaves;
 
  2042             slave < master->slaves + master->slave_count;
 
  2012             slave < master->slaves + master->slave_count;
 
  2043             slave++) {
 
  2013             slave++) {
 
  2044 
  2014 
  2045         // set states for all slaves
 
  2015         // set state to PREOP for all but eoe slaves
 
       
  2016 #ifdef EC_EOE
 
       
  2017         is_eoe_slave = 0;
 
       
  2018         // ... but leave EoE slaves in OP
 
       
  2019         list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
       
  2020             if (slave == eoe->slave && ec_eoe_is_open(eoe))
 
       
  2021                 is_eoe_slave = 1;
 
       
  2022        }
 
       
  2023        if (!is_eoe_slave) {
 
       
  2024            ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
       
  2025            // mark for reconfiguration, because the master could have no
 
       
  2026            // possibility for a reconfiguration between two sequential operation
 
       
  2027            // phases.
 
       
  2028            slave->force_config = 1;
 
       
  2029         }
 
       
  2030 #else
 
  2046         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
  2031         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
 
  2047 
       
  2048         // mark for reconfiguration, because the master could have no
 
  2032         // mark for reconfiguration, because the master could have no
 
  2049         // possibility for a reconfiguration between two sequential operation
 
  2033         // possibility for a reconfiguration between two sequential operation
 
  2050         // phases.
 
  2034         // phases.
 
  2051         slave->force_config = 1;
 
  2035         slave->force_config = 1;
 
  2052     }
 
  2036 #endif
 
  2053 
  2037 
  2054 #ifdef EC_EOE
 
  2038     }
 
  2055     // ... but leave EoE slaves in OP
 
       
  2056     list_for_each_entry(eoe, &master->eoe_handlers, list) {
 
       
  2057         if (ec_eoe_is_open(eoe))
 
       
  2058             ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
 
       
  2059     }
 
       
  2060 #endif
 
       
  2061 
  2039 
  2062     master->app_time = 0ULL;
 
  2040     master->app_time = 0ULL;
 
  2063     master->app_start_time = 0ULL;
 
  2041     master->app_start_time = 0ULL;
 
  2064     master->has_app_time = 0;
 
  2042     master->has_app_time = 0;
 
  2065 
  2043 
  2066 #ifdef EC_EOE
 
       
  2067     if (eoe_was_running) {
 
       
  2068         ec_master_eoe_start(master);
 
       
  2069     }
 
       
  2070 #endif
 
       
  2071     if (ec_master_thread_start(master, ec_master_idle_thread,
 
  2044     if (ec_master_thread_start(master, ec_master_idle_thread,
 
  2072                 "EtherCAT-IDLE"))
 
  2045                 "EtherCAT-IDLE"))
 
  2073         EC_MASTER_WARN(master, "Failed to restart master thread!\n");
 
  2046         EC_MASTER_WARN(master, "Failed to restart master thread!\n");
 
  2074 
  2047 
  2075     master->allow_scan = 1;
 
  2048     master->allow_scan = 1;
 
  2079 
  2052 
  2080 /*****************************************************************************/
 
  2053 /*****************************************************************************/
 
  2081 
  2054 
  2082 void ecrt_master_send(ec_master_t *master)
 
  2055 void ecrt_master_send(ec_master_t *master)
 
  2083 {
 
  2056 {
 
  2084     ec_datagram_t *datagram, *n;
 
  2057     ec_datagram_t *datagram, *next;
 
  2085 
  2058 
  2086     if (master->injection_seq_rt != master->injection_seq_fsm) {
 
  2059     ec_master_inject_fsm_datagrams(master);
 
  2087         // inject datagrams produced by master & slave FSMs
 
       
  2088         ec_master_queue_datagram(master, &master->fsm_datagram);
 
       
  2089         master->injection_seq_rt = master->injection_seq_fsm;
 
       
  2090     }
 
       
  2091     ec_master_inject_external_datagrams(master);
 
       
  2092 
  2060 
  2093     if (unlikely(!master->main_device.link_state)) {
 
  2061     if (unlikely(!master->main_device.link_state)) {
 
  2094         // link is down, no datagram can be sent
 
  2062         // link is down, no datagram can be sent
 
  2095         list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) {
 
  2063         list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) {
 
  2096             datagram->state = EC_DATAGRAM_ERROR;
 
  2064             datagram->state = EC_DATAGRAM_ERROR;
 
  2097             list_del_init(&datagram->queue);
 
  2065             list_del_init(&datagram->queue);
 
  2098         }
 
  2066         }
 
  2099 
  2067 
  2100         // query link state
 
  2068         // query link state
 
  2149             }
 
  2117             }
 
  2150         }
 
  2118         }
 
  2151     }
 
  2119     }
 
  2152 }
 
  2120 }
 
  2153 
  2121 
  2154 /*****************************************************************************/
 
       
  2155 
       
  2156 void ecrt_master_send_ext(ec_master_t *master)
 
       
  2157 {
 
       
  2158     ec_datagram_t *datagram, *next;
 
       
  2159 
       
  2160     list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue,
 
       
  2161             queue) {
 
       
  2162         list_del(&datagram->queue);
 
       
  2163         ec_master_queue_datagram(master, datagram);
 
       
  2164     }
 
       
  2165 
       
  2166     ecrt_master_send(master);
 
       
  2167 }
 
       
  2168 
  2122 
  2169 /*****************************************************************************/
 
  2123 /*****************************************************************************/
 
  2170 
  2124 
  2171 /** Same as ecrt_master_slave_config(), but with ERR_PTR() return value.
 
  2125 /** Same as ecrt_master_slave_config(), but with ERR_PTR() return value.
 
  2172  */
 
  2126  */
 
  2211         }
 
  2165         }
 
  2212 
  2166 
  2213         ec_slave_config_init(sc, master,
 
  2167         ec_slave_config_init(sc, master,
 
  2214                 alias, position, vendor_id, product_code);
 
  2168                 alias, position, vendor_id, product_code);
 
  2215 
  2169 
  2216         down(&master->master_sem);
 
  2170         ec_mutex_lock(&master->master_mutex);
 
  2217 
  2171 
  2218         // try to find the addressed slave
 
  2172         // try to find the addressed slave
 
  2219         ec_slave_config_attach(sc);
 
  2173         ec_slave_config_attach(sc);
 
  2220         ec_slave_config_load_default_sync_config(sc);
 
  2174         ec_slave_config_load_default_sync_config(sc);
 
  2221         list_add_tail(&sc->list, &master->configs);
 
  2175         list_add_tail(&sc->list, &master->configs);
 
  2222 
  2176 
  2223         up(&master->master_sem);
 
  2177         ec_mutex_unlock(&master->master_mutex);
 
  2224     }
 
  2178     }
 
  2225 
  2179 
  2226     return sc;
 
  2180     return sc;
 
  2227 }
 
  2181 }
 
  2228 
  2182 
  2256 int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
 
  2210 int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
 
  2257         ec_slave_info_t *slave_info)
 
  2211         ec_slave_info_t *slave_info)
 
  2258 {
 
  2212 {
 
  2259     const ec_slave_t *slave;
 
  2213     const ec_slave_t *slave;
 
  2260 
  2214 
  2261     if (down_interruptible(&master->master_sem)) {
 
  2215     if (ec_mutex_lock_interruptible(&master->master_mutex)) {
 
  2262         return -EINTR;
 
  2216         return -EINTR;
 
  2263     }
 
  2217     }
 
  2264 
  2218 
  2265     slave = ec_master_find_slave_const(master, 0, slave_position);
 
  2219     slave = ec_master_find_slave_const(master, 0, slave_position);
 
  2266 
  2220 
  2279         strncpy(slave_info->name, slave->sii.name, EC_MAX_STRING_LENGTH);
 
  2233         strncpy(slave_info->name, slave->sii.name, EC_MAX_STRING_LENGTH);
 
  2280     } else {
 
  2234     } else {
 
  2281         slave_info->name[0] = 0;
 
  2235         slave_info->name[0] = 0;
 
  2282     }
 
  2236     }
 
  2283 
  2237 
  2284     up(&master->master_sem);
 
  2238     ec_mutex_unlock(&master->master_mutex);
 
  2285 
  2239 
  2286     return 0;
 
  2240     return 0;
 
  2287 }
 
  2241 }
 
  2288 
  2242 
  2289 /*****************************************************************************/
 
  2243 /*****************************************************************************/
 
  2290 
  2244 
  2291 void ecrt_master_callbacks(ec_master_t *master,
 
  2245 void ecrt_master_callbacks(ec_master_t *master,
 
  2292         void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data)
 
  2246                            void (*lock_cb)(void *), void (*unlock_cb)(void *),
 
  2293 {
 
  2247                            void *cb_data)
 
  2294     EC_MASTER_DBG(master, 1, "ecrt_master_callbacks(master = 0x%p,"
 
  2248 {
 
  2295             " send_cb = 0x%p, receive_cb = 0x%p, cb_data = 0x%p)\n",
 
  2249     EC_MASTER_DBG(master, 1,"ecrt_master_callbacks(master = %p, "
 
  2296             master, send_cb, receive_cb, cb_data);
 
  2250                             "lock_cb = %p, unlock_cb = %p, cb_data = %p)\n",
 
  2297 
  2251                             master, lock_cb, unlock_cb, cb_data);
 
  2298     master->app_send_cb = send_cb;
 
  2252 
  2299     master->app_receive_cb = receive_cb;
 
  2253     master->app_fsm_queue_lock_cb = lock_cb;
 
  2300     master->app_cb_data = cb_data;
 
  2254     master->app_fsm_queue_unlock_cb = unlock_cb;
 
  2301 }
 
  2255     master->app_fsm_queue_locking_data = cb_data;
 
       
  2256 }
 
       
  2257 
  2302 
  2258 
  2303 /*****************************************************************************/
 
  2259 /*****************************************************************************/
 
  2304 
  2260 
  2305 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
 
  2261 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
 
  2306 {
 
  2262 {
 
  2309     state->link_up = master->main_device.link_state;
 
  2265     state->link_up = master->main_device.link_state;
 
  2310 }
 
  2266 }
 
  2311 
  2267 
  2312 /*****************************************************************************/
 
  2268 /*****************************************************************************/
 
  2313 
  2269 
       
  2270 void ecrt_master_configured_slaves_state(const ec_master_t *master, ec_master_state_t *state)
 
       
  2271 {
 
       
  2272     const ec_slave_config_t *sc;
 
       
  2273     ec_slave_config_state_t sc_state;
 
       
  2274 
       
  2275     // collect al_states of all configured online slaves
 
       
  2276     state->al_states = 0;
 
       
  2277     list_for_each_entry(sc, &master->configs, list) {
 
       
  2278         ecrt_slave_config_state(sc,&sc_state);
 
       
  2279         if (sc_state.online)
 
       
  2280             state->al_states |= sc_state.al_state;
 
       
  2281     }
 
       
  2282 
       
  2283     state->slaves_responding = master->fsm.slaves_responding;
 
       
  2284     state->link_up = master->main_device.link_state;
 
       
  2285 }
 
       
  2286 
       
  2287 /*****************************************************************************/
 
       
  2288 
  2314 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 
  2289 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 
  2315 {
 
  2290 {
 
  2316     master->app_time = app_time;
 
  2291     master->app_time = app_time;
 
  2317 
  2292 
  2318     if (unlikely(!master->has_app_time)) {
 
  2293     if (unlikely(!master->has_app_time)) {
 
  2319         master->app_start_time = app_time;
 
  2294 		EC_MASTER_DBG(master, 1, "set application start time = %llu\n",app_time);
 
       
  2295 		master->app_start_time = app_time;
 
       
  2296 #ifdef EC_HAVE_CYCLES
 
       
  2297     master->dc_cycles_app_start_time = get_cycles();
 
       
  2298 #endif
 
       
  2299     master->dc_jiffies_app_start_time = jiffies;
 
  2320         master->has_app_time = 1;
 
  2300         master->has_app_time = 1;
 
  2321     }
 
  2301     }
 
  2322 }
 
  2302 }
 
  2323 
  2303 
  2324 /*****************************************************************************/
 
  2304 /*****************************************************************************/
 
  2360 
  2340 
  2361 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
  2341 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
  2362         uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
 
  2342         uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
 
  2363         uint16_t *error_code)
 
  2343         uint16_t *error_code)
 
  2364 {
 
  2344 {
 
  2365     ec_master_soe_request_t request;
 
  2345     ec_master_soe_request_t* request;
 
  2366     int retval;
 
  2346     int retval;
 
  2367 
  2347 
  2368     if (drive_no > 7) {
 
  2348     if (drive_no > 7) {
 
  2369         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2349         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2370         return -EINVAL;
 
  2350         return -EINVAL;
 
  2371     }
 
  2351     }
 
  2372 
  2352 
  2373     INIT_LIST_HEAD(&request.list);
 
  2353     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2374     ec_soe_request_init(&request.req);
 
  2354     if (!request)
 
  2375     ec_soe_request_set_drive_no(&request.req, drive_no);
 
       
  2376     ec_soe_request_set_idn(&request.req, idn);
 
       
  2377 
       
  2378     if (ec_soe_request_alloc(&request.req, data_size)) {
 
       
  2379         ec_soe_request_clear(&request.req);
 
       
  2380         return -ENOMEM;
 
  2355         return -ENOMEM;
 
  2381     }
 
  2356     kref_init(&request->refcount);
 
  2382 
  2357 
  2383     memcpy(request.req.data, data, data_size);
 
  2358     INIT_LIST_HEAD(&request->list);
 
  2384     request.req.data_size = data_size;
 
  2359     ec_soe_request_init(&request->req);
 
  2385     ec_soe_request_write(&request.req);
 
  2360     ec_soe_request_set_drive_no(&request->req, drive_no);
 
  2386 
  2361     ec_soe_request_set_idn(&request->req, idn);
 
  2387     if (down_interruptible(&master->master_sem))
 
  2362 
       
  2363     if (ec_soe_request_alloc(&request->req, data_size)) {
 
       
  2364         ec_soe_request_clear(&request->req);
 
       
  2365         kref_put(&request->refcount,ec_master_soe_request_release);
 
       
  2366         return -ENOMEM;
 
       
  2367     }
 
       
  2368 
       
  2369     memcpy(request->req.data, data, data_size);
 
       
  2370     request->req.data_size = data_size;
 
       
  2371     ec_soe_request_write(&request->req);
 
       
  2372 
       
  2373     if (ec_mutex_lock_interruptible(&master->master_mutex)) {
 
       
  2374         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2388         return -EINTR;
 
  2375         return -EINTR;
 
  2389 
  2376     }
 
  2390     if (!(request.slave = ec_master_find_slave(
 
  2377 
       
  2378     if (!(request->slave = ec_master_find_slave(
 
  2391                     master, 0, slave_position))) {
 
  2379                     master, 0, slave_position))) {
 
  2392         up(&master->master_sem);
 
  2380         ec_mutex_unlock(&master->master_mutex);
 
  2393         EC_MASTER_ERR(master, "Slave %u does not exist!\n",
 
  2381         EC_MASTER_ERR(master, "Slave %u does not exist!\n",
 
  2394                 slave_position);
 
  2382                 slave_position);
 
  2395         ec_soe_request_clear(&request.req);
 
  2383         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2396         return -EINVAL;
 
  2384         return -EINVAL;
 
  2397     }
 
  2385     }
 
  2398 
  2386 
  2399     EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n");
 
  2387     EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE write request %p.\n",request);
 
  2400 
  2388 
  2401     // schedule SoE write request.
 
  2389     // schedule SoE write request.
 
  2402     list_add_tail(&request.list, &request.slave->soe_requests);
 
  2390     list_add_tail(&request->list, &request->slave->soe_requests);
 
  2403 
  2391     kref_get(&request->refcount);
 
  2404     up(&master->master_sem);
 
  2392 
       
  2393     ec_mutex_unlock(&master->master_mutex);
 
  2405 
  2394 
  2406     // wait for processing through FSM
 
  2395     // wait for processing through FSM
 
  2407     if (wait_event_interruptible(request.slave->soe_queue,
 
  2396     if (wait_event_interruptible(request->slave->soe_queue,
 
  2408                 request.req.state != EC_INT_REQUEST_QUEUED)) {
 
  2397           ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2409         // interrupted by signal
 
  2398            // interrupted by signal
 
  2410         down(&master->master_sem);
 
  2399            kref_put(&request->refcount,ec_master_soe_request_release);
 
  2411         if (request.req.state == EC_INT_REQUEST_QUEUED) {
 
  2400            return -EINTR;
 
  2412             // abort request
 
  2401     }
 
  2413             list_del(&request.list);
 
       
  2414             up(&master->master_sem);
 
       
  2415             ec_soe_request_clear(&request.req);
 
       
  2416             return -EINTR;
 
       
  2417         }
 
       
  2418         up(&master->master_sem);
 
       
  2419     }
 
       
  2420 
       
  2421     // wait until master FSM has finished processing
 
       
  2422     wait_event(request.slave->soe_queue,
 
       
  2423             request.req.state != EC_INT_REQUEST_BUSY);
 
       
  2424 
  2402 
  2425     if (error_code) {
 
  2403     if (error_code) {
 
  2426         *error_code = request.req.error_code;
 
  2404         *error_code = request->req.error_code;
 
  2427     }
 
  2405     }
 
  2428     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
  2406     retval = request->req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
 
  2429     ec_soe_request_clear(&request.req);
 
  2407     kref_put(&request->refcount,ec_master_soe_request_release);
 
  2430 
  2408 
  2431     return retval;
 
  2409     return retval;
 
  2432 }
 
  2410 }
 
  2433 
  2411 
  2434 /*****************************************************************************/
 
  2412 /*****************************************************************************/
 
  2435 
  2413 
  2436 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
 
  2414 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
 
  2437         uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
 
  2415         uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
 
  2438         size_t *result_size, uint16_t *error_code)
 
  2416         size_t *result_size, uint16_t *error_code)
 
  2439 {
 
  2417 {
 
  2440     ec_master_soe_request_t request;
 
  2418     ec_master_soe_request_t* request;
 
  2441 
  2419 
  2442     if (drive_no > 7) {
 
  2420     if (drive_no > 7) {
 
  2443         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2421         EC_MASTER_ERR(master, "Invalid drive number!\n");
 
  2444         return -EINVAL;
 
  2422         return -EINVAL;
 
  2445     }
 
  2423     }
 
  2446 
  2424 
  2447     INIT_LIST_HEAD(&request.list);
 
  2425     request = kmalloc(sizeof(*request), GFP_KERNEL);
 
  2448     ec_soe_request_init(&request.req);
 
  2426     if (!request)
 
  2449     ec_soe_request_set_drive_no(&request.req, drive_no);
 
  2427         return -ENOMEM;
 
  2450     ec_soe_request_set_idn(&request.req, idn);
 
  2428     kref_init(&request->refcount);
 
  2451     ec_soe_request_read(&request.req);
 
  2429 
  2452 
  2430     INIT_LIST_HEAD(&request->list);
 
  2453     if (down_interruptible(&master->master_sem))
 
  2431     ec_soe_request_init(&request->req);
 
       
  2432     ec_soe_request_set_drive_no(&request->req, drive_no);
 
       
  2433     ec_soe_request_set_idn(&request->req, idn);
 
       
  2434     ec_soe_request_read(&request->req);
 
       
  2435 
       
  2436     if (ec_mutex_lock_interruptible(&master->master_mutex)) {
 
       
  2437         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2454         return -EINTR;
 
  2438         return -EINTR;
 
  2455 
  2439     }
 
  2456     if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) {
 
  2440 
  2457         up(&master->master_sem);
 
  2441     if (!(request->slave = ec_master_find_slave(master, 0, slave_position))) {
 
  2458         ec_soe_request_clear(&request.req);
 
  2442         ec_mutex_unlock(&master->master_mutex);
 
       
  2443         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2459         EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
 
  2444         EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
 
  2460         return -EINVAL;
 
  2445         return -EINVAL;
 
  2461     }
 
  2446     }
 
  2462 
  2447 
  2463     // schedule request.
 
  2448     // schedule request.
 
  2464     list_add_tail(&request.list, &request.slave->soe_requests);
 
  2449     list_add_tail(&request->list, &request->slave->soe_requests);
 
  2465 
  2450     kref_get(&request->refcount);
 
  2466     up(&master->master_sem);
 
  2451 
  2467 
  2452     ec_mutex_unlock(&master->master_mutex);
 
  2468     EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n");
 
  2453 
       
  2454     EC_SLAVE_DBG(request->slave, 1, "Scheduled SoE read request %p.\n",request);
 
  2469 
  2455 
  2470     // wait for processing through FSM
 
  2456     // wait for processing through FSM
 
  2471     if (wait_event_interruptible(request.slave->soe_queue,
 
  2457     if (wait_event_interruptible(request->slave->soe_queue,
 
  2472                 request.req.state != EC_INT_REQUEST_QUEUED)) {
 
  2458           ((request->req.state == EC_INT_REQUEST_SUCCESS) || (request->req.state == EC_INT_REQUEST_FAILURE)))) {
 
  2473         // interrupted by signal
 
  2459            // interrupted by signal
 
  2474         down(&master->master_sem);
 
  2460            kref_put(&request->refcount,ec_master_soe_request_release);
 
  2475         if (request.req.state == EC_INT_REQUEST_QUEUED) {
 
  2461            return -EINTR;
 
  2476             list_del(&request.list);
 
  2462     }
 
  2477             up(&master->master_sem);
 
       
  2478             ec_soe_request_clear(&request.req);
 
       
  2479             return -EINTR;
 
       
  2480         }
 
       
  2481         // request already processing: interrupt not possible.
 
       
  2482         up(&master->master_sem);
 
       
  2483     }
 
       
  2484 
       
  2485     // wait until master FSM has finished processing
 
       
  2486     wait_event(request.slave->soe_queue,
 
       
  2487             request.req.state != EC_INT_REQUEST_BUSY);
 
       
  2488 
  2463 
  2489     if (error_code) {
 
  2464     if (error_code) {
 
  2490         *error_code = request.req.error_code;
 
  2465         *error_code = request->req.error_code;
 
  2491     }
 
  2466     }
 
  2492 
  2467 
  2493     EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n",
 
  2468     EC_SLAVE_DBG(request->slave, 1, "SoE request %p read %zd bytes via SoE.\n",
 
  2494             request.req.data_size);
 
  2469             request,request->req.data_size);
 
  2495 
  2470 
  2496     if (request.req.state != EC_INT_REQUEST_SUCCESS) {
 
  2471     if (request->req.state != EC_INT_REQUEST_SUCCESS) {
 
  2497         if (result_size) {
 
  2472         if (result_size) {
 
  2498             *result_size = 0;
 
  2473             *result_size = 0;
 
  2499         }
 
  2474         }
 
  2500         ec_soe_request_clear(&request.req);
 
  2475         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2501         return -EIO;
 
  2476         return -EIO;
 
  2502     } else {
 
  2477     } else {
 
  2503         if (request.req.data_size > target_size) {
 
  2478         if (request->req.data_size > target_size) {
 
  2504             EC_MASTER_ERR(master, "Buffer too small.\n");
 
  2479             EC_MASTER_ERR(master, "Buffer too small.\n");
 
  2505             ec_soe_request_clear(&request.req);
 
  2480             kref_put(&request->refcount,ec_master_soe_request_release);
 
  2506             return -EOVERFLOW;
 
  2481             return -EOVERFLOW;
 
  2507         }
 
  2482         }
 
  2508         if (result_size) {
 
  2483         if (result_size) {
 
  2509             *result_size = request.req.data_size;
 
  2484             *result_size = request->req.data_size;
 
  2510         }
 
  2485         }
 
  2511         memcpy(target, request.req.data, request.req.data_size);
 
  2486         memcpy(target, request->req.data, request->req.data_size);
 
       
  2487         kref_put(&request->refcount,ec_master_soe_request_release);
 
  2512         return 0;
 
  2488         return 0;
 
  2513     }
 
  2489     }
 
  2514 }
 
  2490 }
 
  2515 
  2491 
  2516 /*****************************************************************************/
 
  2492 /*****************************************************************************/
 
  2532 
  2508 
  2533 EXPORT_SYMBOL(ecrt_master_create_domain);
 
  2509 EXPORT_SYMBOL(ecrt_master_create_domain);
 
  2534 EXPORT_SYMBOL(ecrt_master_activate);
 
  2510 EXPORT_SYMBOL(ecrt_master_activate);
 
  2535 EXPORT_SYMBOL(ecrt_master_deactivate);
 
  2511 EXPORT_SYMBOL(ecrt_master_deactivate);
 
  2536 EXPORT_SYMBOL(ecrt_master_send);
 
  2512 EXPORT_SYMBOL(ecrt_master_send);
 
  2537 EXPORT_SYMBOL(ecrt_master_send_ext);
 
       
  2538 EXPORT_SYMBOL(ecrt_master_receive);
 
  2513 EXPORT_SYMBOL(ecrt_master_receive);
 
  2539 EXPORT_SYMBOL(ecrt_master_callbacks);
 
  2514 EXPORT_SYMBOL(ecrt_master_callbacks);
 
  2540 EXPORT_SYMBOL(ecrt_master);
 
  2515 EXPORT_SYMBOL(ecrt_master);
 
  2541 EXPORT_SYMBOL(ecrt_master_get_slave);
 
  2516 EXPORT_SYMBOL(ecrt_master_get_slave);
 
  2542 EXPORT_SYMBOL(ecrt_master_slave_config);
 
  2517 EXPORT_SYMBOL(ecrt_master_slave_config);
etherlabmaster