Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/master.c
branchstable-1.5
changeset 2419 fdb85a806585
parent 2291 1f44094e2004
parent 2380 cf9db49bcce8
child 2421 bc2d4bf9cbe5
equal deleted inserted replaced
2417:63bef67e812b 2419:fdb85a806585 
    80  */
 
    80  */
 
    81 static unsigned long ext_injection_timeout_jiffies;
 
    81 static unsigned long ext_injection_timeout_jiffies;
 
    82 
    82 
    83 #endif
 
    83 #endif
 
    84 
    84 
       
    85 /** List of intervals for statistics [s].
 
       
    86  */
 
       
    87 const unsigned int rate_intervals[] = {
 
       
    88     1, 10, 60
 
       
    89 };
 
       
    90 
    85 /*****************************************************************************/
 
    91 /*****************************************************************************/
 
    86 
    92 
    87 void ec_master_clear_slave_configs(ec_master_t *);
 
    93 void ec_master_clear_slave_configs(ec_master_t *);
 
    88 void ec_master_clear_domains(ec_master_t *);
 
    94 void ec_master_clear_domains(ec_master_t *);
 
    89 static int ec_master_idle_thread(void *);
 
    95 static int ec_master_idle_thread(void *);
 
    90 static int ec_master_operation_thread(void *);
 
    96 static int ec_master_operation_thread(void *);
 
    91 #ifdef EC_EOE
 
    97 #ifdef EC_EOE
 
    92 static int ec_master_eoe_thread(void *);
 
    98 static int ec_master_eoe_thread(void *);
 
    93 #endif
 
    99 #endif
 
    94 void ec_master_find_dc_ref_clock(ec_master_t *);
 
   100 void ec_master_find_dc_ref_clock(ec_master_t *);
 
       
   101 void ec_master_clear_device_stats(ec_master_t *);
 
       
   102 void ec_master_update_device_stats(ec_master_t *);
 
    95 
   103 
    96 /*****************************************************************************/
 
   104 /*****************************************************************************/
 
    97 
   105 
    98 /** Static variables initializer.
 
   106 /** Static variables initializer.
 
    99 */
 
   107 */
 
   100 void ec_master_init_static(void)
 
   108 void ec_master_init_static(void)
 
   101 {
 
   109 {
 
   102 #ifdef EC_HAVE_CYCLES
 
   110 #ifdef EC_HAVE_CYCLES
 
   103     timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
 
   111     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);
 
   112     ext_injection_timeout_cycles =
 
       
   113         (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
 
   105 #else
 
   114 #else
 
   106     // one jiffy may always elapse between time measurement
 
   115     // one jiffy may always elapse between time measurement
 
   107     timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
 
   116     timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
 
   108     ext_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
 
   117     ext_injection_timeout_jiffies =
 
       
   118         max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
 
   109 #endif
 
   119 #endif
 
   110 }
 
   120 }
 
   111 
   121 
   112 /*****************************************************************************/
 
   122 /*****************************************************************************/
 
   113 
   123 
   130     master->index = index;
 
   140     master->index = index;
 
   131     master->reserved = 0;
 
   141     master->reserved = 0;
 
   132 
   142 
   133     sema_init(&master->master_sem, 1);
 
   143     sema_init(&master->master_sem, 1);
 
   134 
   144 
   135     master->main_mac = main_mac;
 
   145     master->macs[EC_DEVICE_MAIN] = main_mac;
 
   136     master->backup_mac = backup_mac;
 
   146     master->macs[EC_DEVICE_BACKUP] = backup_mac;
 
       
   147     ec_master_clear_device_stats(master);
 
   137 
   148 
   138     sema_init(&master->device_sem, 1);
 
   149     sema_init(&master->device_sem, 1);
 
   139 
   150 
   140     master->phase = EC_ORPHANED;
 
   151     master->phase = EC_ORPHANED;
 
   141     master->active = 0;
 
   152     master->active = 0;
 
   199 
   210 
   200     INIT_LIST_HEAD(&master->reg_requests);
 
   211     INIT_LIST_HEAD(&master->reg_requests);
 
   201     init_waitqueue_head(&master->reg_queue);
 
   212     init_waitqueue_head(&master->reg_queue);
 
   202 
   213 
   203     // init devices
 
   214     // init devices
 
   204     ret = ec_device_init(&master->main_device, master);
 
   215     ret = ec_device_init(&master->devices[EC_DEVICE_MAIN], master);
 
   205     if (ret < 0)
 
   216     if (ret < 0)
 
   206         goto out_return;
 
   217         goto out_return;
 
   207 
   218 
   208     ret = ec_device_init(&master->backup_device, master);
 
   219     ret = ec_device_init(&master->devices[EC_DEVICE_BACKUP], master);
 
   209     if (ret < 0)
 
   220     if (ret < 0)
 
   210         goto out_clear_main;
 
   221         goto out_clear_main;
 
   211 
   222 
   212     // init state machine datagram
 
   223     // init state machine datagram
 
   213     ec_datagram_init(&master->fsm_datagram);
 
   224     ec_datagram_init(&master->fsm_datagram);
 
   222     // create state machine object
 
   233     // create state machine object
 
   223     ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
 
   234     ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
 
   224 
   235 
   225     // init reference sync datagram
 
   236     // init reference sync datagram
 
   226     ec_datagram_init(&master->ref_sync_datagram);
 
   237     ec_datagram_init(&master->ref_sync_datagram);
 
   227     snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync");
 
   238     snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE,
 
       
   239             "refsync");
 
   228     ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8);
 
   240     ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8);
 
   229     if (ret < 0) {
 
   241     if (ret < 0) {
 
   230         ec_datagram_clear(&master->ref_sync_datagram);
 
   242         ec_datagram_clear(&master->ref_sync_datagram);
 
   231         EC_MASTER_ERR(master, "Failed to allocate reference"
 
   243         EC_MASTER_ERR(master, "Failed to allocate reference"
 
   232                 " synchronisation datagram.\n");
 
   244                 " synchronisation datagram.\n");
 
   244         goto out_clear_ref_sync;
 
   256         goto out_clear_ref_sync;
 
   245     }
 
   257     }
 
   246 
   258 
   247     // init sync monitor datagram
 
   259     // init sync monitor datagram
 
   248     ec_datagram_init(&master->sync_mon_datagram);
 
   260     ec_datagram_init(&master->sync_mon_datagram);
 
   249     snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon");
 
   261     snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE,
 
       
   262             "syncmon");
 
   250     ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4);
 
   263     ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4);
 
   251     if (ret < 0) {
 
   264     if (ret < 0) {
 
   252         ec_datagram_clear(&master->sync_mon_datagram);
 
   265         ec_datagram_clear(&master->sync_mon_datagram);
 
   253         EC_MASTER_ERR(master, "Failed to allocate sync"
 
   266         EC_MASTER_ERR(master, "Failed to allocate sync"
 
   254                 " monitoring datagram.\n");
 
   267                 " monitoring datagram.\n");
 
   297     ec_datagram_clear(&master->ref_sync_datagram);
 
   310     ec_datagram_clear(&master->ref_sync_datagram);
 
   298 out_clear_fsm:
 
   311 out_clear_fsm:
 
   299     ec_fsm_master_clear(&master->fsm);
 
   312     ec_fsm_master_clear(&master->fsm);
 
   300     ec_datagram_clear(&master->fsm_datagram);
 
   313     ec_datagram_clear(&master->fsm_datagram);
 
   301 out_clear_backup:
 
   314 out_clear_backup:
 
   302     ec_device_clear(&master->backup_device);
 
   315     ec_device_clear(&master->devices[EC_DEVICE_BACKUP]);
 
   303 out_clear_main:
 
   316 out_clear_main:
 
   304     ec_device_clear(&master->main_device);
 
   317     ec_device_clear(&master->devices[EC_DEVICE_MAIN]);
 
   305 out_return:
 
   318 out_return:
 
   306     return ret;
 
   319     return ret;
 
   307 }
 
   320 }
 
   308 
   321 
   309 /*****************************************************************************/
 
   322 /*****************************************************************************/
 
   332     ec_datagram_clear(&master->sync_mon_datagram);
 
   345     ec_datagram_clear(&master->sync_mon_datagram);
 
   333     ec_datagram_clear(&master->sync_datagram);
 
   346     ec_datagram_clear(&master->sync_datagram);
 
   334     ec_datagram_clear(&master->ref_sync_datagram);
 
   347     ec_datagram_clear(&master->ref_sync_datagram);
 
   335     ec_fsm_master_clear(&master->fsm);
 
   348     ec_fsm_master_clear(&master->fsm);
 
   336     ec_datagram_clear(&master->fsm_datagram);
 
   349     ec_datagram_clear(&master->fsm_datagram);
 
   337     ec_device_clear(&master->backup_device);
 
   350     ec_device_clear(&master->devices[EC_DEVICE_BACKUP]);
 
   338     ec_device_clear(&master->main_device);
 
   351     ec_device_clear(&master->devices[EC_DEVICE_MAIN]);
 
   339 }
 
   352 }
 
   340 
   353 
   341 /*****************************************************************************/
 
   354 /*****************************************************************************/
 
   342 
   355 
   343 #ifdef EC_EOE
 
   356 #ifdef EC_EOE
 
   539 int ec_master_enter_idle_phase(
 
   552 int ec_master_enter_idle_phase(
 
   540         ec_master_t *master /**< EtherCAT master */
 
   553         ec_master_t *master /**< EtherCAT master */
 
   541         )
 
   554         )
 
   542 {
 
   555 {
 
   543     int ret;
 
   556     int ret;
 
       
   557     ec_device_index_t dev_idx;
 
   544 
   558 
   545     EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n");
 
   559     EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n");
 
   546 
   560 
   547     master->send_cb = ec_master_internal_send_cb;
 
   561     master->send_cb = ec_master_internal_send_cb;
 
   548     master->receive_cb = ec_master_internal_receive_cb;
 
   562     master->receive_cb = ec_master_internal_receive_cb;
 
   549     master->cb_data = master;
 
   563     master->cb_data = master;
 
   550 
   564 
   551     master->phase = EC_IDLE;
 
   565     master->phase = EC_IDLE;
 
   552 
   566 
   553     // reset number of responding slaves to trigger scanning
 
   567     // reset number of responding slaves to trigger scanning
 
   554     master->fsm.slaves_responding = 0;
 
   568     for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
 
       
   569         master->fsm.slaves_responding[dev_idx] = 0;
 
       
   570     }
 
   555 
   571 
   556     ret = ec_master_thread_start(master, ec_master_idle_thread,
 
   572     ret = ec_master_thread_start(master, ec_master_idle_thread,
 
   557             "EtherCAT-IDLE");
 
   573             "EtherCAT-IDLE");
 
   558     if (ret)
 
   574     if (ret)
 
   559         master->phase = EC_ORPHANED;
 
   575         master->phase = EC_ORPHANED;
 
   583 
   599 
   584 /*****************************************************************************/
 
   600 /*****************************************************************************/
 
   585 
   601 
   586 /** Transition function from IDLE to OPERATION phase.
 
   602 /** Transition function from IDLE to OPERATION phase.
 
   587  */
 
   603  */
 
   588 int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */)
 
   604 int ec_master_enter_operation_phase(
 
       
   605         ec_master_t *master /**< EtherCAT master */
 
       
   606         )
 
   589 {
 
   607 {
 
   590     int ret = 0;
 
   608     int ret = 0;
 
   591     ec_slave_t *slave;
 
   609     ec_slave_t *slave;
 
   592 #ifdef EC_EOE
 
   610 #ifdef EC_EOE
 
   593     ec_eoe_t *eoe;
 
   611     ec_eoe_t *eoe;
 
   620         up(&master->scan_sem);
 
   638         up(&master->scan_sem);
 
   621     } else {
 
   639     } else {
 
   622         up(&master->scan_sem);
 
   640         up(&master->scan_sem);
 
   623 
   641 
   624         // wait for slave scan to complete
 
   642         // wait for slave scan to complete
 
   625         ret = wait_event_interruptible(master->scan_queue, !master->scan_busy);
 
   643         ret = wait_event_interruptible(master->scan_queue,
 
       
   644                 !master->scan_busy);
 
   626         if (ret) {
 
   645         if (ret) {
 
   627             EC_MASTER_INFO(master, "Waiting for slave scan"
 
   646             EC_MASTER_INFO(master, "Waiting for slave scan"
 
   628                     " interrupted by signal.\n");
 
   647                     " interrupted by signal.\n");
 
   629             goto out_allow;
 
   648             goto out_allow;
 
   630         }
 
   649         }
 
   710 #ifdef EC_HAVE_CYCLES
 
   729 #ifdef EC_HAVE_CYCLES
 
   711             datagram->cycles_sent = 0;
 
   730             datagram->cycles_sent = 0;
 
   712 #endif
 
   731 #endif
 
   713             datagram->jiffies_sent = 0;
 
   732             datagram->jiffies_sent = 0;
 
   714             ec_master_queue_datagram(master, datagram);
 
   733             ec_master_queue_datagram(master, datagram);
 
   715         }
 
   734         } else {
 
   716         else {
 
       
   717             if (datagram->data_size > master->max_queue_size) {
 
   735             if (datagram->data_size > master->max_queue_size) {
 
   718                 list_del_init(&datagram->queue);
 
   736                 list_del_init(&datagram->queue);
 
   719                 datagram->state = EC_DATAGRAM_ERROR;
 
   737                 datagram->state = EC_DATAGRAM_ERROR;
 
   720                 EC_MASTER_ERR(master, "External datagram %p is too large,"
 
   738                 EC_MASTER_ERR(master, "External datagram %p is too large,"
 
   721                         " size=%zu, max_queue_size=%zu\n",
 
   739                         " size=%zu, max_queue_size=%zu\n",
 
   878     up(&master->ext_queue_sem);
 
   896     up(&master->ext_queue_sem);
 
   879 }
 
   897 }
 
   880 
   898 
   881 /*****************************************************************************/
 
   899 /*****************************************************************************/
 
   882 
   900 
   883 /** Sends the datagrams in the queue.
 
   901 /** Sends the datagrams in the queue for a certain device.
 
   884  *
 
   902  *
 
   885  */
 
   903  */
 
   886 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
 
   904 void ec_master_send_datagrams(
 
       
   905         ec_master_t *master, /**< EtherCAT master */
 
       
   906         ec_device_index_t device_index /**< Device index. */
 
       
   907         )
 
   887 {
 
   908 {
 
   888     ec_datagram_t *datagram, *next;
 
   909     ec_datagram_t *datagram, *next;
 
   889     size_t datagram_size;
 
   910     size_t datagram_size;
 
   890     uint8_t *frame_data, *cur_data;
 
   911     uint8_t *frame_data, *cur_data = NULL;
 
   891     void *follows_word;
 
   912     void *follows_word;
 
   892 #ifdef EC_HAVE_CYCLES
 
   913 #ifdef EC_HAVE_CYCLES
 
   893     cycles_t cycles_start, cycles_sent, cycles_end;
 
   914     cycles_t cycles_start, cycles_sent, cycles_end;
 
   894 #endif
 
   915 #endif
 
   895     unsigned long jiffies_sent;
 
   916     unsigned long jiffies_sent;
 
   900     cycles_start = get_cycles();
 
   921     cycles_start = get_cycles();
 
   901 #endif
 
   922 #endif
 
   902     frame_count = 0;
 
   923     frame_count = 0;
 
   903     INIT_LIST_HEAD(&sent_datagrams);
 
   924     INIT_LIST_HEAD(&sent_datagrams);
 
   904 
   925 
   905     EC_MASTER_DBG(master, 2, "ec_master_send_datagrams\n");
 
   926     EC_MASTER_DBG(master, 2, "%s(device_index = %u)\n",
 
       
   927             __func__, device_index);
 
   906 
   928 
   907     do {
 
   929     do {
 
   908         // fetch pointer to transmit socket buffer
 
   930         frame_data = NULL;
 
   909         frame_data = ec_device_tx_data(&master->main_device);
 
       
   910         cur_data = frame_data + EC_FRAME_HEADER_SIZE;
 
       
   911         follows_word = NULL;
 
   931         follows_word = NULL;
 
   912         more_datagrams_waiting = 0;
 
   932         more_datagrams_waiting = 0;
 
   913 
   933 
   914         // fill current frame with datagrams
 
   934         // fill current frame with datagrams
 
   915         list_for_each_entry(datagram, &master->datagram_queue, queue) {
 
   935         list_for_each_entry(datagram, &master->datagram_queue, queue) {
 
   916             if (datagram->state != EC_DATAGRAM_QUEUED) continue;
 
   936             if (datagram->state != EC_DATAGRAM_QUEUED ||
 
       
   937                     datagram->device_index != device_index) {
 
       
   938                 continue;
 
       
   939             }
 
       
   940 
       
   941             if (!frame_data) {
 
       
   942                 // fetch pointer to transmit socket buffer
 
       
   943                 frame_data =
 
       
   944                     ec_device_tx_data(&master->devices[device_index]);
 
       
   945                 cur_data = frame_data + EC_FRAME_HEADER_SIZE;
 
       
   946             }
 
   917 
   947 
   918             // does the current datagram fit in the frame?
 
   948             // does the current datagram fit in the frame?
 
   919             datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
 
   949             datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
 
   920                 + EC_DATAGRAM_FOOTER_SIZE;
 
   950                 + EC_DATAGRAM_FOOTER_SIZE;
 
   921             if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) {
 
   951             if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) {
 
   924             }
 
   954             }
 
   925 
   955 
   926             list_add_tail(&datagram->sent, &sent_datagrams);
 
   956             list_add_tail(&datagram->sent, &sent_datagrams);
 
   927             datagram->index = master->datagram_index++;
 
   957             datagram->index = master->datagram_index++;
 
   928 
   958 
   929             EC_MASTER_DBG(master, 2, "adding datagram 0x%02X\n",
 
   959             EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n",
 
   930                     datagram->index);
 
   960                     datagram->index);
 
   931 
   961 
   932             // set "datagram following" flag in previous frame
 
   962             // set "datagram following" flag in previous datagram
 
   933             if (follows_word)
 
   963             if (follows_word) {
 
   934                 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);
 
   964                 EC_WRITE_U16(follows_word,
 
       
   965                         EC_READ_U16(follows_word) | 0x8000);
 
       
   966             }
 
   935 
   967 
   936             // EtherCAT datagram header
 
   968             // EtherCAT datagram header
 
   937             EC_WRITE_U8 (cur_data,     datagram->type);
 
   969             EC_WRITE_U8 (cur_data, datagram->type);
 
   938             EC_WRITE_U8 (cur_data + 1, datagram->index);
 
   970             EC_WRITE_U8 (cur_data + 1, datagram->index);
 
   939             memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN);
 
   971             memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN);
 
   940             EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
 
   972             EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
 
   941             EC_WRITE_U16(cur_data + 8, 0x0000);
 
   973             EC_WRITE_U16(cur_data + 8, 0x0000);
 
   942             follows_word = cur_data + 6;
 
   974             follows_word = cur_data + 6;
 
   956             break;
 
   988             break;
 
   957         }
 
   989         }
 
   958 
   990 
   959         // EtherCAT frame header
 
   991         // EtherCAT frame header
 
   960         EC_WRITE_U16(frame_data, ((cur_data - frame_data
 
   992         EC_WRITE_U16(frame_data, ((cur_data - frame_data
 
   961                                    - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);
 
   993                         - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);
 
   962 
   994 
   963         // pad frame
 
   995         // pad frame
 
   964         while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN)
 
   996         while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN)
 
   965             EC_WRITE_U8(cur_data++, 0x00);
 
   997             EC_WRITE_U8(cur_data++, 0x00);
 
   966 
   998 
   967         EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data);
 
   999         EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data);
 
   968 
  1000 
   969         // send frame
 
  1001         // send frame
 
   970         ec_device_send(&master->main_device, cur_data - frame_data);
 
  1002         ec_device_send(&master->devices[device_index],
 
       
  1003                 cur_data - frame_data);
 
   971 #ifdef EC_HAVE_CYCLES
 
  1004 #ifdef EC_HAVE_CYCLES
 
   972         cycles_sent = get_cycles();
 
  1005         cycles_sent = get_cycles();
 
   973 #endif
 
  1006 #endif
 
   974         jiffies_sent = jiffies;
 
  1007         jiffies_sent = jiffies;
 
   975 
  1008 
   988     while (more_datagrams_waiting);
 
  1021     while (more_datagrams_waiting);
 
   989 
  1022 
   990 #ifdef EC_HAVE_CYCLES
 
  1023 #ifdef EC_HAVE_CYCLES
 
   991     if (unlikely(master->debug_level > 1)) {
 
  1024     if (unlikely(master->debug_level > 1)) {
 
   992         cycles_end = get_cycles();
 
  1025         cycles_end = get_cycles();
 
   993         EC_MASTER_DBG(master, 0, "ec_master_send_datagrams"
 
  1026         EC_MASTER_DBG(master, 0, "%s()"
 
   994                 " sent %u frames in %uus.\n", frame_count,
 
  1027                 " sent %u frames in %uus.\n", __func__, frame_count,
 
   995                (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz);
 
  1028                (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz);
 
   996     }
 
  1029     }
 
   997 #endif
 
  1030 #endif
 
   998 }
 
  1031 }
 
   999 
  1032 
  1088                 EC_MASTER_DBG(master, 0, "UNMATCHED datagram:\n");
 
  1121                 EC_MASTER_DBG(master, 0, "UNMATCHED datagram:\n");
 
  1089                 ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE,
 
  1122                 ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE,
 
  1090                         EC_DATAGRAM_HEADER_SIZE + data_size
 
  1123                         EC_DATAGRAM_HEADER_SIZE + data_size
 
  1091                         + EC_DATAGRAM_FOOTER_SIZE);
 
  1124                         + EC_DATAGRAM_FOOTER_SIZE);
 
  1092 #ifdef EC_DEBUG_RING
 
  1125 #ifdef EC_DEBUG_RING
 
  1093                 ec_device_debug_ring_print(&master->main_device);
 
  1126                 ec_device_debug_ring_print(&master->devices[EC_DEVICE_MAIN]);
 
  1094 #endif
 
  1127 #endif
 
  1095             }
 
  1128             }
 
  1096 
  1129 
  1097             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
 
  1130             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
 
  1098             continue;
 
  1131             continue;
 
  1113         cur_data += EC_DATAGRAM_FOOTER_SIZE;
 
  1146         cur_data += EC_DATAGRAM_FOOTER_SIZE;
 
  1114 
  1147 
  1115         // dequeue the received datagram
 
  1148         // dequeue the received datagram
 
  1116         datagram->state = EC_DATAGRAM_RECEIVED;
 
  1149         datagram->state = EC_DATAGRAM_RECEIVED;
 
  1117 #ifdef EC_HAVE_CYCLES
 
  1150 #ifdef EC_HAVE_CYCLES
 
  1118         datagram->cycles_received = master->main_device.cycles_poll;
 
  1151         datagram->cycles_received =
 
  1119 #endif
 
  1152             master->devices[EC_DEVICE_MAIN].cycles_poll;
 
  1120         datagram->jiffies_received = master->main_device.jiffies_poll;
 
  1153 #endif
 
       
  1154         datagram->jiffies_received =
 
       
  1155             master->devices[EC_DEVICE_MAIN].jiffies_poll;
 
  1121         list_del_init(&datagram->queue);
 
  1156         list_del_init(&datagram->queue);
 
  1122     }
 
  1157     }
 
  1123 }
 
  1158 }
 
  1124 
  1159 
  1125 /*****************************************************************************/
 
  1160 /*****************************************************************************/
 
  1153             master->stats.unmatched = 0;
 
  1188             master->stats.unmatched = 0;
 
  1154         }
 
  1189         }
 
  1155     }
 
  1190     }
 
  1156 }
 
  1191 }
 
  1157 
  1192 
       
  1193 /*****************************************************************************/
 
       
  1194 
       
  1195 /** Clears the common device statistics.
 
       
  1196  */
 
       
  1197 void ec_master_clear_device_stats(
 
       
  1198         ec_master_t *master /**< EtherCAT master */
 
       
  1199         )
 
       
  1200 {
 
       
  1201     unsigned int i;
 
       
  1202 
       
  1203     // zero frame statistics
 
       
  1204     master->device_stats.tx_count = 0;
 
       
  1205     master->device_stats.last_tx_count = 0;
 
       
  1206     master->device_stats.rx_count = 0;
 
       
  1207     master->device_stats.last_rx_count = 0;
 
       
  1208     master->device_stats.tx_bytes = 0;
 
       
  1209     master->device_stats.last_tx_bytes = 0;
 
       
  1210     master->device_stats.rx_bytes = 0;
 
       
  1211     master->device_stats.last_rx_bytes = 0;
 
       
  1212     master->device_stats.last_loss = 0;
 
       
  1213 
       
  1214     for (i = 0; i < EC_RATE_COUNT; i++) {
 
       
  1215         master->device_stats.tx_frame_rates[i] = 0;
 
       
  1216         master->device_stats.tx_byte_rates[i] = 0;
 
       
  1217         master->device_stats.loss_rates[i] = 0;
 
       
  1218     }
 
       
  1219 }
 
       
  1220 
       
  1221 /*****************************************************************************/
 
       
  1222 
       
  1223 /** Updates the common device statistics.
 
       
  1224  */
 
       
  1225 void ec_master_update_device_stats(
 
       
  1226         ec_master_t *master /**< EtherCAT master */
 
       
  1227         )
 
       
  1228 {
 
       
  1229     ec_device_stats_t *s = &master->device_stats;
 
       
  1230     s32 tx_frame_rate, rx_frame_rate, tx_byte_rate, rx_byte_rate, loss_rate;
 
       
  1231     u64 loss;
 
       
  1232     unsigned int i;
 
       
  1233 
       
  1234     // frame statistics
 
       
  1235     if (likely(jiffies - s->jiffies < HZ)) {
 
       
  1236         return;
 
       
  1237     }
 
       
  1238 
       
  1239     tx_frame_rate = (s->tx_count - s->last_tx_count) * 1000;
 
       
  1240     rx_frame_rate = (s->rx_count - s->last_rx_count) * 1000;
 
       
  1241     tx_byte_rate = s->tx_bytes - s->last_tx_bytes;
 
       
  1242     rx_byte_rate = s->rx_bytes - s->last_rx_bytes;
 
       
  1243     loss = s->tx_count - s->rx_count;
 
       
  1244     loss_rate = (loss - s->last_loss) * 1000;
 
       
  1245 
       
  1246     /* Low-pass filter:
 
       
  1247      *      Y_n = y_(n - 1) + T / tau * (x - y_(n - 1))   | T = 1
 
       
  1248      *   -> Y_n += (x - y_(n - 1)) / tau
 
       
  1249      */
 
       
  1250     for (i = 0; i < EC_RATE_COUNT; i++) {
 
       
  1251         s32 n = rate_intervals[i];
 
       
  1252         s->tx_frame_rates[i] += (tx_frame_rate - s->tx_frame_rates[i]) / n;
 
       
  1253         s->rx_frame_rates[i] += (rx_frame_rate - s->rx_frame_rates[i]) / n;
 
       
  1254         s->tx_byte_rates[i] += (tx_byte_rate - s->tx_byte_rates[i]) / n;
 
       
  1255         s->rx_byte_rates[i] += (rx_byte_rate - s->rx_byte_rates[i]) / n;
 
       
  1256         s->loss_rates[i] += (loss_rate - s->loss_rates[i]) / n;
 
       
  1257     }
 
       
  1258 
       
  1259     s->last_tx_count = s->tx_count;
 
       
  1260     s->last_rx_count = s->rx_count;
 
       
  1261     s->last_tx_bytes = s->tx_bytes;
 
       
  1262     s->last_rx_bytes = s->rx_bytes;
 
       
  1263     s->last_loss = loss;
 
       
  1264 
       
  1265     ec_device_update_stats(&master->devices[EC_DEVICE_MAIN]);
 
       
  1266     ec_device_update_stats(&master->devices[EC_DEVICE_BACKUP]);
 
       
  1267 
       
  1268     s->jiffies = jiffies;
 
       
  1269 }
 
  1158 
  1270 
  1159 /*****************************************************************************/
 
  1271 /*****************************************************************************/
 
  1160 
  1272 
  1161 #ifdef EC_USE_HRTIMER
 
  1273 #ifdef EC_USE_HRTIMER
 
  1162 
  1274 
  1276         if (fsm_exec) {
 
  1388         if (fsm_exec) {
 
  1277             ec_master_queue_datagram(master, &master->fsm_datagram);
 
  1389             ec_master_queue_datagram(master, &master->fsm_datagram);
 
  1278         }
 
  1390         }
 
  1279         ecrt_master_send(master);
 
  1391         ecrt_master_send(master);
 
  1280 #ifdef EC_USE_HRTIMER
 
  1392 #ifdef EC_USE_HRTIMER
 
  1281         sent_bytes = master->main_device.tx_skb[
 
  1393         sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[
 
  1282             master->main_device.tx_ring_index]->len;
 
  1394             master->devices[EC_DEVICE_MAIN].tx_ring_index]->len;
 
  1283 #endif
 
  1395 #endif
 
  1284         up(&master->io_sem);
 
  1396         up(&master->io_sem);
 
  1285 
  1397 
  1286         if (ec_fsm_master_idle(&master->fsm)) {
 
  1398         if (ec_fsm_master_idle(&master->fsm)) {
 
  1287 #ifdef EC_USE_HRTIMER
 
  1399 #ifdef EC_USE_HRTIMER
 
  1930     unsigned int index;
 
  2042     unsigned int index;
 
  1931 
  2043 
  1932     EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n",
 
  2044     EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n",
 
  1933             master);
 
  2045             master);
 
  1934 
  2046 
  1935     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
  2047     if (!(domain =
 
       
  2048                 (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
  1936         EC_MASTER_ERR(master, "Error allocating domain memory!\n");
 
  2049         EC_MASTER_ERR(master, "Error allocating domain memory!\n");
 
  1937         return ERR_PTR(-ENOMEM);
 
  2050         return ERR_PTR(-ENOMEM);
 
  1938     }
 
  2051     }
 
  1939 
  2052 
  1940     down(&master->master_sem);
 
  2053     down(&master->master_sem);
 
  2113 /*****************************************************************************/
 
  2226 /*****************************************************************************/
 
  2114 
  2227 
  2115 void ecrt_master_send(ec_master_t *master)
 
  2228 void ecrt_master_send(ec_master_t *master)
 
  2116 {
 
  2229 {
 
  2117     ec_datagram_t *datagram, *n;
 
  2230     ec_datagram_t *datagram, *n;
 
       
  2231     ec_device_index_t dev_idx;
 
  2118 
  2232 
  2119     if (master->injection_seq_rt != master->injection_seq_fsm) {
 
  2233     if (master->injection_seq_rt != master->injection_seq_fsm) {
 
  2120         // inject datagrams produced by master & slave FSMs
 
  2234         // inject datagrams produced by master and slave FSMs
 
  2121         ec_master_queue_datagram(master, &master->fsm_datagram);
 
  2235         ec_master_queue_datagram(master, &master->fsm_datagram);
 
  2122         master->injection_seq_rt = master->injection_seq_fsm;
 
  2236         master->injection_seq_rt = master->injection_seq_fsm;
 
  2123     }
 
  2237     }
 
  2124 
  2238 
  2125     ec_master_inject_external_datagrams(master);
 
  2239     ec_master_inject_external_datagrams(master);
 
  2126 
  2240 
  2127     if (unlikely(!master->main_device.link_state)) {
 
  2241     for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
 
  2128         // link is down, no datagram can be sent
 
  2242         if (unlikely(!master->devices[dev_idx].link_state)) {
 
  2129         list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) {
 
  2243             // link is down, no datagram can be sent
 
  2130             datagram->state = EC_DATAGRAM_ERROR;
 
  2244             list_for_each_entry_safe(datagram, n,
 
  2131             list_del_init(&datagram->queue);
 
  2245                     &master->datagram_queue, queue) {
 
  2132         }
 
  2246                 if (datagram->device_index == dev_idx) {
 
  2133 
  2247                     datagram->state = EC_DATAGRAM_ERROR;
 
  2134         // query link state
 
  2248                     list_del_init(&datagram->queue);
 
  2135         ec_device_poll(&master->main_device);
 
  2249                 }
 
  2136 
  2250             }
 
  2137         // clear frame statistics
 
  2251 
  2138         ec_device_clear_stats(&master->main_device);
 
  2252             if (!master->devices[dev_idx].dev) {
 
  2139         return;
 
  2253                 continue;
 
  2140     }
 
  2254             }
 
  2141 
  2255 
  2142     // send frames
 
  2256             // query link state
 
  2143     ec_master_send_datagrams(master);
 
  2257             ec_device_poll(&master->devices[dev_idx]);
 
       
  2258 
       
  2259             // clear frame statistics
 
       
  2260             ec_device_clear_stats(&master->devices[dev_idx]);
 
       
  2261             continue;
 
       
  2262         }
 
       
  2263 
       
  2264         // send frames
 
       
  2265         ec_master_send_datagrams(master, dev_idx);
 
       
  2266     }
 
  2144 }
 
  2267 }
 
  2145 
  2268 
  2146 /*****************************************************************************/
 
  2269 /*****************************************************************************/
 
  2147 
  2270 
  2148 void ecrt_master_receive(ec_master_t *master)
 
  2271 void ecrt_master_receive(ec_master_t *master)
 
  2149 {
 
  2272 {
 
  2150     ec_datagram_t *datagram, *next;
 
  2273     ec_datagram_t *datagram, *next;
 
  2151 
  2274 
  2152     // receive datagrams
 
  2275     // receive datagrams
 
  2153     ec_device_poll(&master->main_device);
 
  2276     ec_device_poll(&master->devices[EC_DEVICE_MAIN]);
 
       
  2277     if (master->devices[EC_DEVICE_BACKUP].dev) {
 
       
  2278         ec_device_poll(&master->devices[EC_DEVICE_BACKUP]);
 
       
  2279     }
 
       
  2280     ec_master_update_device_stats(master);
 
  2154 
  2281 
  2155     // dequeue all datagrams that timed out
 
  2282     // dequeue all datagrams that timed out
 
  2156     list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) {
 
  2283     list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) {
 
  2157         if (datagram->state != EC_DATAGRAM_SENT) continue;
 
  2284         if (datagram->state != EC_DATAGRAM_SENT) continue;
 
  2158 
  2285 
  2159 #ifdef EC_HAVE_CYCLES
 
  2286 #ifdef EC_HAVE_CYCLES
 
  2160         if (master->main_device.cycles_poll - datagram->cycles_sent
 
  2287         if (master->devices[EC_DEVICE_MAIN].cycles_poll -
 
  2161                 > timeout_cycles) {
 
  2288                 datagram->cycles_sent > timeout_cycles) {
 
  2162 #else
 
  2289 #else
 
  2163         if (master->main_device.jiffies_poll - datagram->jiffies_sent
 
  2290         if (master->devices[EC_DEVICE_MAIN].jiffies_poll -
 
  2164                 > timeout_jiffies) {
 
  2291                 datagram->jiffies_sent > timeout_jiffies) {
 
  2165 #endif
 
  2292 #endif
 
  2166             list_del_init(&datagram->queue);
 
  2293             list_del_init(&datagram->queue);
 
  2167             datagram->state = EC_DATAGRAM_TIMED_OUT;
 
  2294             datagram->state = EC_DATAGRAM_TIMED_OUT;
 
  2168             master->stats.timeouts++;
 
  2295             master->stats.timeouts++;
 
  2169             ec_master_output_stats(master);
 
  2296             ec_master_output_stats(master);
 
  2170 
  2297 
  2171             if (unlikely(master->debug_level > 0)) {
 
  2298             if (unlikely(master->debug_level > 0)) {
 
  2172                 unsigned int time_us;
 
  2299                 unsigned int time_us;
 
  2173 #ifdef EC_HAVE_CYCLES
 
  2300 #ifdef EC_HAVE_CYCLES
 
  2174                 time_us = (unsigned int) (master->main_device.cycles_poll -
 
  2301                 time_us = (unsigned int)
 
       
  2302                     (master->devices[EC_DEVICE_MAIN].cycles_poll -
 
  2175                         datagram->cycles_sent) * 1000 / cpu_khz;
 
  2303                         datagram->cycles_sent) * 1000 / cpu_khz;
 
  2176 #else
 
  2304 #else
 
  2177                 time_us = (unsigned int) ((master->main_device.jiffies_poll -
 
  2305                 time_us = (unsigned int)
 
       
  2306                     ((master->devices[EC_DEVICE_MAIN].jiffies_poll -
 
  2178                             datagram->jiffies_sent) * 1000000 / HZ);
 
  2307                             datagram->jiffies_sent) * 1000000 / HZ);
 
  2179 #endif
 
  2308 #endif
 
  2180                 EC_MASTER_DBG(master, 0, "TIMED OUT datagram %p,"
 
  2309                 EC_MASTER_DBG(master, 0, "TIMED OUT datagram %p,"
 
  2181                         " index %02X waited %u us.\n",
 
  2310                         " index %02X waited %u us.\n",
 
  2182                         datagram, datagram->index, time_us);
 
  2311                         datagram, datagram->index, time_us);
 
  2277 {
 
  2406 {
 
  2278     EC_MASTER_DBG(master, 1, "ecrt_master(master = 0x%p,"
 
  2407     EC_MASTER_DBG(master, 1, "ecrt_master(master = 0x%p,"
 
  2279             " master_info = 0x%p)\n", master, master_info);
 
  2408             " master_info = 0x%p)\n", master, master_info);
 
  2280 
  2409 
  2281     master_info->slave_count = master->slave_count;
 
  2410     master_info->slave_count = master->slave_count;
 
  2282     master_info->link_up = master->main_device.link_state;
 
  2411     master_info->link_up = master->devices[EC_DEVICE_MAIN].link_state;
 
  2283     master_info->scan_busy = master->scan_busy;
 
  2412     master_info->scan_busy = master->scan_busy;
 
  2284     master_info->app_time = master->app_time;
 
  2413     master_info->app_time = master->app_time;
 
  2285     return 0;
 
  2414     return 0;
 
  2286 }
 
  2415 }
 
  2287 
  2416 
  2356 
  2485 
  2357 /*****************************************************************************/
 
  2486 /*****************************************************************************/
 
  2358 
  2487 
  2359 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
 
  2488 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
 
  2360 {
 
  2489 {
 
  2361     state->slaves_responding = master->fsm.slaves_responding;
 
  2490     ec_device_index_t dev_idx;
 
  2362     state->al_states = master->fsm.slave_states;
 
  2491 
  2363     state->link_up = master->main_device.link_state;
 
  2492     state->slaves_responding = 0U;
 
       
  2493     state->al_states = 0;
 
       
  2494     state->link_up = 0U;
 
       
  2495 
       
  2496     for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_NUM_DEVICES; dev_idx++) {
 
       
  2497         /* Announce sum of responding slaves on all links. */
 
       
  2498         state->slaves_responding += master->fsm.slaves_responding[dev_idx];
 
       
  2499 
       
  2500         /* Binary-or slave states of all links. */
 
       
  2501         state->al_states |= master->fsm.slave_states[dev_idx];
 
       
  2502 
       
  2503         /* Signal link up if at least one device has link. */
 
       
  2504         state->link_up |= master->devices[dev_idx].link_state;
 
       
  2505     }
 
       
  2506 }
 
       
  2507 
       
  2508 /*****************************************************************************/
 
       
  2509 
       
  2510 int ecrt_master_link_state(const ec_master_t *master, unsigned int dev_idx,
 
       
  2511         ec_master_link_state_t *state)
 
       
  2512 {
 
       
  2513     if (dev_idx >= EC_NUM_DEVICES) {
 
       
  2514         return -EINVAL;
 
       
  2515     }
 
       
  2516 
       
  2517     state->slaves_responding = master->fsm.slaves_responding[dev_idx];
 
       
  2518     state->al_states = master->fsm.slave_states[dev_idx];
 
       
  2519     state->link_up = master->devices[dev_idx].link_state;
 
       
  2520 
       
  2521     return 0;
 
  2364 }
 
  2522 }
 
  2365 
  2523 
  2366 /*****************************************************************************/
 
  2524 /*****************************************************************************/
 
  2367 
  2525 
  2368 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 
  2526 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 
  2836 EXPORT_SYMBOL(ecrt_master_callbacks);
 
  2994 EXPORT_SYMBOL(ecrt_master_callbacks);
 
  2837 EXPORT_SYMBOL(ecrt_master);
 
  2995 EXPORT_SYMBOL(ecrt_master);
 
  2838 EXPORT_SYMBOL(ecrt_master_get_slave);
 
  2996 EXPORT_SYMBOL(ecrt_master_get_slave);
 
  2839 EXPORT_SYMBOL(ecrt_master_slave_config);
 
  2997 EXPORT_SYMBOL(ecrt_master_slave_config);
 
  2840 EXPORT_SYMBOL(ecrt_master_state);
 
  2998 EXPORT_SYMBOL(ecrt_master_state);
 
       
  2999 EXPORT_SYMBOL(ecrt_master_link_state);
 
  2841 EXPORT_SYMBOL(ecrt_master_application_time);
 
  3000 EXPORT_SYMBOL(ecrt_master_application_time);
 
  2842 EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
 
  3001 EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
 
  2843 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
 
  3002 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
 
  2844 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
 
  3003 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
 
  2845 EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
 
  3004 EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
etherlabmaster