52 (result)->tv_usec += 1000000; \ |
52 (result)->tv_usec += 1000000; \ |
53 } \ |
53 } \ |
54 } while (0) |
54 } while (0) |
55 #endif |
55 #endif |
56 |
56 |
57 /** List of intervals for frame statistics [s]. |
|
58 */ |
|
59 static const unsigned int rate_intervals[] = { |
|
60 1, 10, 60 |
|
61 }; |
|
62 |
|
63 /*****************************************************************************/ |
57 /*****************************************************************************/ |
64 |
58 |
65 /** Constructor. |
59 /** Constructor. |
66 * |
60 * |
67 * \return 0 in case of success, else < 0 |
61 * \return 0 in case of success, else < 0 |
68 */ |
62 */ |
69 int ec_device_init( |
63 int ec_device_init( |
70 ec_device_t *device, /**< EtherCAT device */ |
64 ec_device_t *device, /**< EtherCAT device */ |
71 ec_master_t *master /**< master owning the device */ |
65 ec_master_t *master /**< master owning the device */ |
76 struct ethhdr *eth; |
70 struct ethhdr *eth; |
77 #ifdef EC_DEBUG_IF |
71 #ifdef EC_DEBUG_IF |
78 char ifname[10]; |
72 char ifname[10]; |
79 char mb = 'x'; |
73 char mb = 'x'; |
80 #endif |
74 #endif |
|
75 |
|
76 device->master = master; |
|
77 device->dev = NULL; |
|
78 device->poll = NULL; |
|
79 device->module = NULL; |
|
80 device->open = 0; |
|
81 device->link_state = 0; |
|
82 for (i = 0; i < EC_TX_RING_SIZE; i++) { |
|
83 device->tx_skb[i] = NULL; |
|
84 } |
|
85 device->tx_ring_index = 0; |
|
86 #ifdef EC_HAVE_CYCLES |
|
87 device->cycles_poll = 0; |
|
88 #endif |
|
89 #ifdef EC_DEBUG_RING |
|
90 device->timeval_poll.tv_sec = 0; |
|
91 device->timeval_poll.tv_usec = 0; |
|
92 #endif |
|
93 device->jiffies_poll = 0; |
|
94 |
|
95 ec_device_clear_stats(device); |
|
96 |
|
97 #ifdef EC_DEBUG_RING |
|
98 for (i = 0; i < EC_DEBUG_RING_SIZE; i++) { |
|
99 ec_debug_frame_t *df = &device->debug_frames[i]; |
|
100 df->dir = TX; |
|
101 df->t.tv_sec = 0; |
|
102 df->t.tv_usec = 0; |
|
103 memset(df->data, 0, EC_MAX_DATA_SIZE); |
|
104 df->data_size = 0; |
|
105 } |
|
106 #endif |
81 #ifdef EC_DEBUG_RING |
107 #ifdef EC_DEBUG_RING |
82 device->debug_frame_index = 0; |
108 device->debug_frame_index = 0; |
83 device->debug_frame_count = 0; |
109 device->debug_frame_count = 0; |
84 #endif |
110 #endif |
85 |
111 |
86 device->master = master; |
|
87 device->tx_ring_index = 0; |
|
88 |
|
89 #ifdef EC_DEBUG_IF |
112 #ifdef EC_DEBUG_IF |
90 if (device == &master->main_device) |
113 if (device == &master->devices[EC_DEVICE_MAIN]) { |
91 mb = 'm'; |
114 mb = 'm'; |
92 else if (device == &master->backup_device) |
115 } |
|
116 else { |
93 mb = 'b'; |
117 mb = 'b'; |
|
118 } |
94 |
119 |
95 sprintf(ifname, "ecdbg%c%u", mb, master->index); |
120 sprintf(ifname, "ecdbg%c%u", mb, master->index); |
96 |
121 |
97 ret = ec_debug_init(&device->dbg, device, ifname); |
122 ret = ec_debug_init(&device->dbg, device, ifname); |
98 if (ret < 0) { |
123 if (ret < 0) { |
99 EC_MASTER_ERR(master, "Failed to init debug device!\n"); |
124 EC_MASTER_ERR(master, "Failed to init debug device!\n"); |
100 goto out_return; |
125 goto out_return; |
101 } |
126 } |
102 #endif |
127 #endif |
103 |
|
104 for (i = 0; i < EC_TX_RING_SIZE; i++) |
|
105 device->tx_skb[i] = NULL; |
|
106 |
128 |
107 for (i = 0; i < EC_TX_RING_SIZE; i++) { |
129 for (i = 0; i < EC_TX_RING_SIZE; i++) { |
108 if (!(device->tx_skb[i] = dev_alloc_skb(ETH_FRAME_LEN))) { |
130 if (!(device->tx_skb[i] = dev_alloc_skb(ETH_FRAME_LEN))) { |
109 EC_MASTER_ERR(master, "Error allocating device socket buffer!\n"); |
131 EC_MASTER_ERR(master, "Error allocating device socket buffer!\n"); |
110 ret = -ENOMEM; |
132 ret = -ENOMEM; |
116 eth = (struct ethhdr *) skb_push(device->tx_skb[i], ETH_HLEN); |
138 eth = (struct ethhdr *) skb_push(device->tx_skb[i], ETH_HLEN); |
117 eth->h_proto = htons(0x88A4); |
139 eth->h_proto = htons(0x88A4); |
118 memset(eth->h_dest, 0xFF, ETH_ALEN); |
140 memset(eth->h_dest, 0xFF, ETH_ALEN); |
119 } |
141 } |
120 |
142 |
121 ec_device_detach(device); // resets remaining fields |
|
122 return 0; |
143 return 0; |
123 |
144 |
124 out_tx_ring: |
145 out_tx_ring: |
125 for (i = 0; i < EC_TX_RING_SIZE; i++) |
146 for (i = 0; i < EC_TX_RING_SIZE; i++) { |
126 if (device->tx_skb[i]) |
147 if (device->tx_skb[i]) { |
127 dev_kfree_skb(device->tx_skb[i]); |
148 dev_kfree_skb(device->tx_skb[i]); |
|
149 } |
|
150 } |
128 #ifdef EC_DEBUG_IF |
151 #ifdef EC_DEBUG_IF |
129 ec_debug_clear(&device->dbg); |
152 ec_debug_clear(&device->dbg); |
130 out_return: |
153 out_return: |
131 #endif |
154 #endif |
132 return ret; |
155 return ret; |
201 device->open = 0; |
226 device->open = 0; |
202 device->link_state = 0; // down |
227 device->link_state = 0; // down |
203 |
228 |
204 ec_device_clear_stats(device); |
229 ec_device_clear_stats(device); |
205 |
230 |
206 for (i = 0; i < EC_TX_RING_SIZE; i++) |
231 for (i = 0; i < EC_TX_RING_SIZE; i++) { |
207 device->tx_skb[i]->dev = NULL; |
232 device->tx_skb[i]->dev = NULL; |
|
233 } |
208 } |
234 } |
209 |
235 |
210 /*****************************************************************************/ |
236 /*****************************************************************************/ |
211 |
237 |
212 /** Opens the EtherCAT device. |
238 /** Opens the EtherCAT device. |
307 size_t size /**< number of bytes to send */ |
333 size_t size /**< number of bytes to send */ |
308 ) |
334 ) |
309 { |
335 { |
310 struct sk_buff *skb = device->tx_skb[device->tx_ring_index]; |
336 struct sk_buff *skb = device->tx_skb[device->tx_ring_index]; |
311 |
337 |
312 // frame statistics |
|
313 if (unlikely(jiffies - device->stats_jiffies >= HZ)) { |
|
314 unsigned int i; |
|
315 u32 tx_frame_rate = |
|
316 (u32) (device->tx_count - device->last_tx_count) * 1000; |
|
317 u32 tx_byte_rate = |
|
318 (device->tx_bytes - device->last_tx_bytes); |
|
319 u64 loss = device->tx_count - device->rx_count; |
|
320 s32 loss_rate = (s32) (loss - device->last_loss) * 1000; |
|
321 for (i = 0; i < EC_RATE_COUNT; i++) { |
|
322 unsigned int n = rate_intervals[i]; |
|
323 device->tx_frame_rates[i] = |
|
324 (device->tx_frame_rates[i] * (n - 1) + tx_frame_rate) / n; |
|
325 device->tx_byte_rates[i] = |
|
326 (device->tx_byte_rates[i] * (n - 1) + tx_byte_rate) / n; |
|
327 device->loss_rates[i] = |
|
328 (device->loss_rates[i] * (n - 1) + loss_rate) / n; |
|
329 } |
|
330 device->last_tx_count = device->tx_count; |
|
331 device->last_tx_bytes = device->tx_bytes; |
|
332 device->last_loss = loss; |
|
333 device->stats_jiffies = jiffies; |
|
334 } |
|
335 |
|
336 // set the right length for the data |
338 // set the right length for the data |
337 skb->len = ETH_HLEN + size; |
339 skb->len = ETH_HLEN + size; |
338 |
340 |
339 if (unlikely(device->master->debug_level > 1)) { |
341 if (unlikely(device->master->debug_level > 1)) { |
340 EC_MASTER_DBG(device->master, 2, "Sending frame:\n"); |
342 EC_MASTER_DBG(device->master, 2, "Sending frame:\n"); |
348 #else |
350 #else |
349 if (device->dev->hard_start_xmit(skb, device->dev) == NETDEV_TX_OK) |
351 if (device->dev->hard_start_xmit(skb, device->dev) == NETDEV_TX_OK) |
350 #endif |
352 #endif |
351 { |
353 { |
352 device->tx_count++; |
354 device->tx_count++; |
|
355 device->master->device_stats.tx_count++; |
353 device->tx_bytes += ETH_HLEN + size; |
356 device->tx_bytes += ETH_HLEN + size; |
|
357 device->master->device_stats.tx_bytes += ETH_HLEN + size; |
354 #ifdef EC_DEBUG_IF |
358 #ifdef EC_DEBUG_IF |
355 ec_debug_send(&device->dbg, skb->data, ETH_HLEN + size); |
359 ec_debug_send(&device->dbg, skb->data, ETH_HLEN + size); |
356 #endif |
360 #endif |
357 #ifdef EC_DEBUG_RING |
361 #ifdef EC_DEBUG_RING |
358 ec_device_debug_ring_append( |
362 ec_device_debug_ring_append( |
373 { |
377 { |
374 unsigned int i; |
378 unsigned int i; |
375 |
379 |
376 // zero frame statistics |
380 // zero frame statistics |
377 device->tx_count = 0; |
381 device->tx_count = 0; |
|
382 device->last_tx_count = 0; |
378 device->rx_count = 0; |
383 device->rx_count = 0; |
|
384 device->last_rx_count = 0; |
|
385 device->tx_bytes = 0; |
|
386 device->last_tx_bytes = 0; |
|
387 device->rx_bytes = 0; |
|
388 device->last_rx_bytes = 0; |
379 device->tx_errors = 0; |
389 device->tx_errors = 0; |
380 device->tx_bytes = 0; |
390 |
381 device->last_tx_count = 0; |
|
382 device->last_tx_bytes = 0; |
|
383 device->last_loss = 0; |
|
384 for (i = 0; i < EC_RATE_COUNT; i++) { |
391 for (i = 0; i < EC_RATE_COUNT; i++) { |
385 device->tx_frame_rates[i] = 0; |
392 device->tx_frame_rates[i] = 0; |
|
393 device->rx_frame_rates[i] = 0; |
386 device->tx_byte_rates[i] = 0; |
394 device->tx_byte_rates[i] = 0; |
387 device->loss_rates[i] = 0; |
395 device->rx_byte_rates[i] = 0; |
388 } |
396 } |
389 } |
397 } |
390 |
398 |
391 /*****************************************************************************/ |
399 /*****************************************************************************/ |
392 |
400 |
401 ) |
409 ) |
402 { |
410 { |
403 ec_debug_frame_t *df = &device->debug_frames[device->debug_frame_index]; |
411 ec_debug_frame_t *df = &device->debug_frames[device->debug_frame_index]; |
404 |
412 |
405 df->dir = dir; |
413 df->dir = dir; |
406 if (dir == TX) |
414 if (dir == TX) { |
407 do_gettimeofday(&df->t); |
415 do_gettimeofday(&df->t); |
408 else |
416 } |
|
417 else { |
409 df->t = device->timeval_poll; |
418 df->t = device->timeval_poll; |
|
419 } |
410 memcpy(df->data, data, size); |
420 memcpy(df->data, data, size); |
411 df->data_size = size; |
421 df->data_size = size; |
412 |
422 |
413 device->debug_frame_index++; |
423 device->debug_frame_index++; |
414 device->debug_frame_index %= EC_DEBUG_RING_SIZE; |
424 device->debug_frame_index %= EC_DEBUG_RING_SIZE; |
477 do_gettimeofday(&device->timeval_poll); |
487 do_gettimeofday(&device->timeval_poll); |
478 #endif |
488 #endif |
479 device->poll(device->dev); |
489 device->poll(device->dev); |
480 } |
490 } |
481 |
491 |
|
492 /*****************************************************************************/ |
|
493 |
|
494 /** Update device statistics. |
|
495 */ |
|
496 void ec_device_update_stats( |
|
497 ec_device_t *device /**< EtherCAT device */ |
|
498 ) |
|
499 { |
|
500 unsigned int i; |
|
501 |
|
502 s32 tx_frame_rate = (device->tx_count - device->last_tx_count) * 1000; |
|
503 s32 rx_frame_rate = (device->rx_count - device->last_rx_count) * 1000; |
|
504 s32 tx_byte_rate = (device->tx_bytes - device->last_tx_bytes); |
|
505 s32 rx_byte_rate = (device->rx_bytes - device->last_rx_bytes); |
|
506 |
|
507 /* Low-pass filter: |
|
508 * Y_n = y_(n - 1) + T / tau * (x - y_(n - 1)) | T = 1 |
|
509 * -> Y_n += (x - y_(n - 1)) / tau |
|
510 */ |
|
511 for (i = 0; i < EC_RATE_COUNT; i++) { |
|
512 s32 n = rate_intervals[i]; |
|
513 device->tx_frame_rates[i] += |
|
514 (tx_frame_rate - device->tx_frame_rates[i]) / n; |
|
515 device->rx_frame_rates[i] += |
|
516 (rx_frame_rate - device->rx_frame_rates[i]) / n; |
|
517 device->tx_byte_rates[i] += |
|
518 (tx_byte_rate - device->tx_byte_rates[i]) / n; |
|
519 device->rx_byte_rates[i] += |
|
520 (rx_byte_rate - device->rx_byte_rates[i]) / n; |
|
521 } |
|
522 |
|
523 device->last_tx_count = device->tx_count; |
|
524 device->last_rx_count = device->rx_count; |
|
525 device->last_tx_bytes = device->tx_bytes; |
|
526 device->last_rx_bytes = device->rx_bytes; |
|
527 } |
|
528 |
482 /****************************************************************************** |
529 /****************************************************************************** |
483 * Device interface |
530 * Device interface |
484 *****************************************************************************/ |
531 *****************************************************************************/ |
485 |
532 |
486 /** Withdraws an EtherCAT device from the master. |
533 /** Withdraws an EtherCAT device from the master. |
494 * \ingroup DeviceInterface |
541 * \ingroup DeviceInterface |
495 */ |
542 */ |
496 void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */) |
543 void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */) |
497 { |
544 { |
498 ec_master_t *master = device->master; |
545 ec_master_t *master = device->master; |
499 char str[20]; |
546 char dev_str[20], mac_str[20]; |
500 |
547 |
501 ec_mac_print(device->dev->dev_addr, str); |
548 ec_mac_print(device->dev->dev_addr, mac_str); |
502 EC_MASTER_INFO(master, "Releasing main device %s.\n", str); |
549 |
503 |
550 if (device == &master->devices[EC_DEVICE_MAIN]) { |
504 ec_mutex_lock(&master->device_mutex); |
551 sprintf(dev_str, "main"); |
|
552 } else if (device == &master->devices[EC_DEVICE_BACKUP]) { |
|
553 sprintf(dev_str, "backup"); |
|
554 } else { |
|
555 EC_MASTER_WARN(master, "%s() called with unknown device %s!\n", |
|
556 __func__, mac_str); |
|
557 sprintf(dev_str, "UNKNOWN"); |
|
558 } |
|
559 |
|
560 EC_MASTER_INFO(master, "Releasing %s device %s.\n", dev_str, mac_str); |
|
561 |
|
562 down(&master->device_sem); |
505 ec_device_detach(device); |
563 ec_device_detach(device); |
506 ec_mutex_unlock(&master->device_mutex); |
564 up(&master->device_sem); |
507 } |
565 } |
508 |
566 |
509 /*****************************************************************************/ |
567 /*****************************************************************************/ |
510 |
568 |
511 /** Opens the network device and makes the master enter IDLE phase. |
569 /** Opens the network device and makes the master enter IDLE phase. |
514 * \ingroup DeviceInterface |
572 * \ingroup DeviceInterface |
515 */ |
573 */ |
516 int ecdev_open(ec_device_t *device /**< EtherCAT device */) |
574 int ecdev_open(ec_device_t *device /**< EtherCAT device */) |
517 { |
575 { |
518 int ret; |
576 int ret; |
|
577 ec_master_t *master = device->master; |
|
578 unsigned int all_open = 1, dev_idx; |
519 |
579 |
520 ret = ec_device_open(device); |
580 ret = ec_device_open(device); |
521 if (ret) { |
581 if (ret) { |
522 EC_MASTER_ERR(device->master, "Failed to open device!\n"); |
582 EC_MASTER_ERR(master, "Failed to open device!\n"); |
523 return ret; |
583 return ret; |
524 } |
584 } |
525 |
585 |
526 ret = ec_master_enter_idle_phase(device->master); |
586 for (dev_idx = EC_DEVICE_MAIN; |
527 if (ret) { |
587 dev_idx < ec_master_num_devices(device->master); dev_idx++) { |
528 EC_MASTER_ERR(device->master, "Failed to enter IDLE phase!\n"); |
588 if (!master->devices[dev_idx].open) { |
529 return ret; |
589 all_open = 0; |
|
590 break; |
|
591 } |
|
592 } |
|
593 |
|
594 if (all_open) { |
|
595 ret = ec_master_enter_idle_phase(device->master); |
|
596 if (ret) { |
|
597 EC_MASTER_ERR(device->master, "Failed to enter IDLE phase!\n"); |
|
598 return ret; |
|
599 } |
530 } |
600 } |
531 |
601 |
532 return 0; |
602 return 0; |
533 } |
603 } |
534 |
604 |
539 * \return 0 on success, else < 0 |
609 * \return 0 on success, else < 0 |
540 * \ingroup DeviceInterface |
610 * \ingroup DeviceInterface |
541 */ |
611 */ |
542 void ecdev_close(ec_device_t *device /**< EtherCAT device */) |
612 void ecdev_close(ec_device_t *device /**< EtherCAT device */) |
543 { |
613 { |
544 ec_master_leave_idle_phase(device->master); |
614 ec_master_t *master = device->master; |
545 |
615 |
546 if (ec_device_close(device)) |
616 if (master->phase == EC_IDLE) { |
547 EC_MASTER_WARN(device->master, "Failed to close device!\n"); |
617 ec_master_leave_idle_phase(master); |
|
618 } |
|
619 |
|
620 if (ec_device_close(device)) { |
|
621 EC_MASTER_WARN(master, "Failed to close device!\n"); |
|
622 } |
548 } |
623 } |
549 |
624 |
550 /*****************************************************************************/ |
625 /*****************************************************************************/ |
551 |
626 |
552 /** Accepts a received frame. |
627 /** Accepts a received frame. |
553 * |
628 * |
554 * Forwards the received data to the master. The master will analyze the frame |
629 * Forwards the received data to the master. The master will analyze the frame |
555 * and dispatch the received commands to the sending instances. |
630 * and dispatch the received commands to the sending instances. |
556 * |
631 * |
557 * \ingroup DeviceInterface |
632 * \ingroup DeviceInterface |
558 */ |
633 */ |
559 void ecdev_receive( |
634 void ecdev_receive( |
560 ec_device_t *device, /**< EtherCAT device */ |
635 ec_device_t *device, /**< EtherCAT device */ |
561 const void *data, /**< pointer to received data */ |
636 const void *data, /**< pointer to received data */ |
583 #endif |
661 #endif |
584 #ifdef EC_DEBUG_RING |
662 #ifdef EC_DEBUG_RING |
585 ec_device_debug_ring_append(device, RX, ec_data, ec_size); |
663 ec_device_debug_ring_append(device, RX, ec_data, ec_size); |
586 #endif |
664 #endif |
587 |
665 |
588 ec_master_receive_datagrams(device->master, ec_data, ec_size); |
666 ec_master_receive_datagrams(device->master, device, ec_data, ec_size); |
589 } |
667 } |
590 |
668 |
591 /*****************************************************************************/ |
669 /*****************************************************************************/ |
592 |
670 |
593 /** Sets a new link state. |
671 /** Sets a new link state. |
594 * |
672 * |
595 * If the device notifies the master about the link being down, the master |
673 * If the device notifies the master about the link being down, the master |
596 * will not try to send frames using this device. |
674 * will not try to send frames using this device. |
597 * |
675 * |
598 * \ingroup DeviceInterface |
676 * \ingroup DeviceInterface |
599 */ |
677 */ |
600 void ecdev_set_link( |
678 void ecdev_set_link( |
601 ec_device_t *device, /**< EtherCAT device */ |
679 ec_device_t *device, /**< EtherCAT device */ |
602 uint8_t state /**< new link state */ |
680 uint8_t state /**< new link state */ |
603 ) |
681 ) |
604 { |
682 { |
605 if (unlikely(!device)) { |
683 if (unlikely(!device)) { |
606 EC_MASTER_WARN(device->master, "ecdev_set_link(): No device!\n"); |
684 EC_WARN("ecdev_set_link() called with null device!\n"); |
607 return; |
685 return; |
608 } |
686 } |
609 |
687 |
610 if (likely(state != device->link_state)) { |
688 if (likely(state != device->link_state)) { |
611 device->link_state = state; |
689 device->link_state = state; |
612 EC_MASTER_INFO(device->master, |
690 EC_MASTER_INFO(device->master, |
613 "Link state changed to %s.\n", (state ? "UP" : "DOWN")); |
691 "Link state of %s changed to %s.\n", |
|
692 device->dev->name, (state ? "UP" : "DOWN")); |
614 } |
693 } |
615 } |
694 } |
616 |
695 |
617 /*****************************************************************************/ |
696 /*****************************************************************************/ |
618 |
697 |
619 /** Reads the link state. |
698 /** Reads the link state. |
620 * |
699 * |
621 * \ingroup DeviceInterface |
700 * \ingroup DeviceInterface |
|
701 * |
|
702 * \return Link state. |
622 */ |
703 */ |
623 uint8_t ecdev_get_link( |
704 uint8_t ecdev_get_link( |
624 const ec_device_t *device /**< EtherCAT device */ |
705 const ec_device_t *device /**< EtherCAT device */ |
625 ) |
706 ) |
626 { |
707 { |
627 if (unlikely(!device)) { |
708 if (unlikely(!device)) { |
628 EC_MASTER_WARN(device->master, "ecdev_get_link(): No device!\n"); |
709 EC_WARN("ecdev_get_link() called with null device!\n"); |
629 return 0; |
710 return 0; |
630 } |
711 } |
631 |
712 |
632 return device->link_state; |
713 return device->link_state; |
633 } |
714 } |