aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/net/ethernet/freescale/fec_ptp.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/net/ethernet/freescale/fec_ptp.c')
-rw-r--r--drivers/net/ethernet/freescale/fec_ptp.c641
1 files changed, 600 insertions, 41 deletions
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 5007e4f9fff9..06c679ebcd9c 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -1,7 +1,7 @@
1/* 1/*
2 * Fast Ethernet Controller (ENET) PTP driver for MX6x. 2 * Fast Ethernet Controller (ENET) PTP driver for MX6x.
3 * 3 *
4 * Copyright (C) 2012 Freescale Semiconductor, Inc. 4 * Copyright (C) 2012-2013 Freescale Semiconductor, Inc.
5 * 5 *
6 * This program is free software; you can redistribute it and/or modify it 6 * This program is free software; you can redistribute it and/or modify it
7 * under the terms and conditions of the GNU General Public License, 7 * under the terms and conditions of the GNU General Public License,
@@ -37,6 +37,9 @@
37#include <linux/workqueue.h> 37#include <linux/workqueue.h>
38#include <linux/bitops.h> 38#include <linux/bitops.h>
39#include <linux/io.h> 39#include <linux/io.h>
40#include <linux/vmalloc.h>
41#include <linux/ip.h>
42#include <linux/udp.h>
40#include <linux/irq.h> 43#include <linux/irq.h>
41#include <linux/clk.h> 44#include <linux/clk.h>
42#include <linux/platform_device.h> 45#include <linux/platform_device.h>
@@ -49,6 +52,7 @@
49 52
50#include "fec.h" 53#include "fec.h"
51 54
55
52/* FEC 1588 register bits */ 56/* FEC 1588 register bits */
53#define FEC_T_CTRL_SLAVE 0x00002000 57#define FEC_T_CTRL_SLAVE 0x00002000
54#define FEC_T_CTRL_CAPTURE 0x00000800 58#define FEC_T_CTRL_CAPTURE 0x00000800
@@ -71,6 +75,446 @@
71#define FEC_TS_TIMESTAMP 0x418 75#define FEC_TS_TIMESTAMP 0x418
72 76
73#define FEC_CC_MULT (1 << 31) 77#define FEC_CC_MULT (1 << 31)
78
79/* Alloc the ring resource */
80static int fec_ptp_init_circ(struct fec_ptp_circular *buf, int size)
81{
82 buf->data_buf = (struct fec_ptp_ts_data *)
83 vmalloc(size * sizeof(struct fec_ptp_ts_data));
84
85 if (!buf->data_buf)
86 return 1;
87 buf->front = 0;
88 buf->end = 0;
89 buf->size = size;
90 return 0;
91}
92
93static inline int fec_ptp_calc_index(int size, int curr_index, int offset)
94{
95 return (curr_index + offset) % size;
96}
97
98static int fec_ptp_is_empty(struct fec_ptp_circular *buf)
99{
100 return (buf->front == buf->end);
101}
102
103static int fec_ptp_nelems(struct fec_ptp_circular *buf)
104{
105 const int front = buf->front;
106 const int end = buf->end;
107 const int size = buf->size;
108 int n_items;
109
110 if (end > front)
111 n_items = end - front;
112 else if (end < front)
113 n_items = size - (front - end);
114 else
115 n_items = 0;
116
117 return n_items;
118}
119
120static int fec_ptp_is_full(struct fec_ptp_circular *buf)
121{
122 if (fec_ptp_nelems(buf) == (buf->size - 1))
123 return 1;
124 else
125 return 0;
126}
127
128static int fec_ptp_insert(struct fec_ptp_circular *ptp_buf,
129 struct fec_ptp_ts_data *data)
130{
131 struct fec_ptp_ts_data *tmp;
132
133 if (fec_ptp_is_full(ptp_buf))
134 ptp_buf->end = fec_ptp_calc_index(ptp_buf->size,
135 ptp_buf->end, 1);
136
137 tmp = (ptp_buf->data_buf + ptp_buf->end);
138 memcpy(tmp, data, sizeof(struct fec_ptp_ts_data));
139 ptp_buf->end = fec_ptp_calc_index(ptp_buf->size, ptp_buf->end, 1);
140
141 return 0;
142}
143
144static int fec_ptp_find_and_remove(struct fec_ptp_circular *ptp_buf,
145 struct fec_ptp_ident *ident, struct ptp_time *ts)
146{
147 int i;
148 int size = ptp_buf->size, end = ptp_buf->end;
149 struct fec_ptp_ident *tmp_ident;
150
151 if (fec_ptp_is_empty(ptp_buf))
152 return 1;
153
154 i = ptp_buf->front;
155 while (i != end) {
156 tmp_ident = &(ptp_buf->data_buf + i)->ident;
157 if (tmp_ident->version == ident->version) {
158 if (tmp_ident->message_type == ident->message_type) {
159 if ((tmp_ident->netw_prot == ident->netw_prot)
160 || (ident->netw_prot ==
161 FEC_PTP_PROT_DONTCARE)) {
162 if (tmp_ident->seq_id ==
163 ident->seq_id) {
164 int ret =
165 memcmp(tmp_ident->spid,
166 ident->spid,
167 PTP_SOURCE_PORT_LENGTH);
168 if (0 == ret)
169 break;
170 }
171 }
172 }
173 }
174 /* get next */
175 i = fec_ptp_calc_index(size, i, 1);
176 }
177
178 /* not found ? */
179 if (i == end) {
180 /* buffer full ? */
181 if (fec_ptp_is_full(ptp_buf))
182 /* drop one in front */
183 ptp_buf->front =
184 fec_ptp_calc_index(size, ptp_buf->front, 1);
185
186 return 1;
187 }
188 *ts = (ptp_buf->data_buf + i)->ts;
189 ptp_buf->front = fec_ptp_calc_index(size, ptp_buf->front, 1);
190
191 return 0;
192}
193
194/* 1588 Module intialization */
195void fec_ptp_start(struct net_device *ndev)
196{
197 struct fec_enet_private *fep = netdev_priv(ndev);
198 unsigned long flags;
199 int inc;
200
201 inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(fep->clk_ptp);
202
203 spin_lock_irqsave(&fep->tmreg_lock, flags);
204 /* Select 1588 Timer source and enable module for starting Tmr Clock */
205 writel(FEC_T_CTRL_RESTART, fep->hwp + FEC_ATIME_CTRL);
206 writel(inc << FEC_T_INC_OFFSET,
207 fep->hwp + FEC_ATIME_INC);
208 writel(FEC_T_PERIOD_ONE_SEC, fep->hwp + FEC_ATIME_EVT_PERIOD);
209 /* start counter */
210 writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE,
211 fep->hwp + FEC_ATIME_CTRL);
212 spin_unlock_irqrestore(&fep->tmreg_lock, flags);
213}
214
215/* Cleanup routine for 1588 module.
216 * When PTP is disabled this routing is called */
217void fec_ptp_stop(struct net_device *ndev)
218{
219 struct fec_enet_private *priv = netdev_priv(ndev);
220
221 writel(0, priv->hwp + FEC_ATIME_CTRL);
222 writel(FEC_T_CTRL_RESTART, priv->hwp + FEC_ATIME_CTRL);
223}
224
225static void fec_get_curr_cnt(struct fec_enet_private *priv,
226 struct ptp_rtc_time *curr_time)
227{
228 u32 tempval;
229
230 tempval = readl(priv->hwp + FEC_ATIME_CTRL);
231 tempval |= FEC_T_CTRL_CAPTURE;
232
233 writel(tempval, priv->hwp + FEC_ATIME_CTRL);
234 curr_time->rtc_time.nsec = readl(priv->hwp + FEC_ATIME);
235 curr_time->rtc_time.sec = priv->prtc;
236
237 writel(tempval, priv->hwp + FEC_ATIME_CTRL);
238 tempval = readl(priv->hwp + FEC_ATIME);
239
240 if (tempval < curr_time->rtc_time.nsec) {
241 curr_time->rtc_time.nsec = tempval;
242 curr_time->rtc_time.sec = priv->prtc;
243 }
244}
245
246/* Set the 1588 timer counter registers */
247static void fec_set_1588cnt(struct fec_enet_private *priv,
248 struct ptp_rtc_time *fec_time)
249{
250 u32 tempval;
251 unsigned long flags;
252
253 spin_lock_irqsave(&priv->tmreg_lock, flags);
254 priv->prtc = fec_time->rtc_time.sec;
255
256 tempval = fec_time->rtc_time.nsec;
257 writel(tempval, priv->hwp + FEC_ATIME);
258 spin_unlock_irqrestore(&priv->tmreg_lock, flags);
259}
260
261/**
262 * Parse packets if they are PTP.
263 * The PTP header can be found in an IPv4, IPv6 or in an IEEE802.3
264 * ethernet frame. The function returns the position of the PTP packet
265 * or NULL, if no PTP found
266 */
267u8 *fec_ptp_parse_packet(struct sk_buff *skb, u16 *eth_type)
268{
269 u8 *position = skb->data + ETH_ALEN + ETH_ALEN;
270 u8 *ptp_loc = NULL;
271
272 *eth_type = *((u16 *)position);
273 /* Check if outer vlan tag is here */
274 if (ntohs(*eth_type) == ETH_P_8021Q) {
275 position += FEC_VLAN_TAG_LEN;
276 *eth_type = *((u16 *)position);
277 }
278
279 /* set position after ethertype */
280 position += FEC_ETHTYPE_LEN;
281 if (ETH_P_1588 == ntohs(*eth_type)) {
282 ptp_loc = position;
283 /* IEEE1588 event message which needs timestamping */
284 if ((ptp_loc[0] & 0xF) <= 3) {
285 if (skb->len >=
286 ((ptp_loc - skb->data) + PTP_HEADER_SZE))
287 return ptp_loc;
288 }
289 } else if (ETH_P_IP == ntohs(*eth_type)) {
290 u8 *ip_header, *prot, *udp_header;
291 u8 ip_version, ip_hlen;
292 ip_header = position;
293 ip_version = ip_header[0] >> 4; /* correct IP version? */
294 if (0x04 == ip_version) { /* IPv4 */
295 prot = ip_header + 9; /* protocol */
296 if (FEC_PACKET_TYPE_UDP == *prot) {
297 u16 udp_dstPort;
298 /* retrieve the size of the ip-header
299 * with the first byte of the ip-header:
300 * version ( 4 bits) + Internet header
301 * length (4 bits)
302 */
303 ip_hlen = (*ip_header & 0xf) * 4;
304 udp_header = ip_header + ip_hlen;
305 udp_dstPort = *((u16 *)(udp_header + 2));
306 /* check the destination port address
307 * ( 319 (0x013F) = PTP event port )
308 */
309 if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
310 ptp_loc = udp_header + 8;
311 /* long enough ? */
312 if (skb->len >= ((ptp_loc - skb->data)
313 + PTP_HEADER_SZE))
314 return ptp_loc;
315 }
316 }
317 }
318 } else if (ETH_P_IPV6 == ntohs(*eth_type)) {
319 u8 *ip_header, *udp_header, *prot;
320 u8 ip_version;
321 ip_header = position;
322 ip_version = ip_header[0] >> 4;
323 if (0x06 == ip_version) {
324 prot = ip_header + 6;
325 if (FEC_PACKET_TYPE_UDP == *prot) {
326 u16 udp_dstPort;
327 udp_header = ip_header + 40;
328 udp_dstPort = *((u16 *)(udp_header + 2));
329 /* check the destination port address
330 * ( 319 (0x013F) = PTP event port )
331 */
332 if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
333 ptp_loc = udp_header + 8;
334 /* long enough ? */
335 if (skb->len >= ((ptp_loc - skb->data)
336 + PTP_HEADER_SZE))
337 return ptp_loc;
338 }
339 }
340 }
341 }
342
343 return NULL; /* no PTP frame */
344}
345
346/* Set the BD to ptp */
347int fec_ptp_do_txstamp(struct sk_buff *skb)
348{
349 u8 *ptp_loc;
350 u16 eth_type;
351
352 ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
353 if (ptp_loc != NULL)
354 return 1;
355
356 return 0;
357}
358
359void fec_ptp_store_txstamp(struct fec_enet_private *priv,
360 struct sk_buff *skb,
361 struct bufdesc *bdp)
362{
363 struct fec_ptp_ts_data tmp_tx_time;
364 struct bufdesc_ex *bdp_ex = NULL;
365 u8 *ptp_loc;
366 u16 eth_type;
367
368 bdp_ex = container_of(bdp, struct bufdesc_ex, desc);
369 ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
370 if (ptp_loc != NULL) {
371 /* store identification data */
372 switch (ntohs(eth_type)) {
373 case ETH_P_IP:
374 tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
375 break;
376 case ETH_P_IPV6:
377 tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
378 break;
379 case ETH_P_1588:
380 tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
381 break;
382 default:
383 return;
384 }
385 tmp_tx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
386 tmp_tx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
387 tmp_tx_time.ident.seq_id =
388 ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
389 memcpy(tmp_tx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
390 PTP_SOURCE_PORT_LENGTH);
391 /* store tx timestamp */
392 tmp_tx_time.ts.sec = priv->prtc;
393 tmp_tx_time.ts.nsec = bdp_ex->ts;
394 /* insert timestamp in circular buffer */
395 fec_ptp_insert(&(priv->tx_timestamps), &tmp_tx_time);
396 }
397}
398
399void fec_ptp_store_rxstamp(struct fec_enet_private *priv,
400 struct sk_buff *skb,
401 struct bufdesc *bdp)
402{
403 struct fec_ptp_ts_data tmp_rx_time;
404 struct bufdesc_ex *bdp_ex = NULL;
405 u8 *ptp_loc;
406 u16 eth_type;
407
408 bdp_ex = container_of(bdp, struct bufdesc_ex, desc);
409 ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
410 if (ptp_loc != NULL) {
411 /* store identification data */
412 tmp_rx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
413 tmp_rx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
414 switch (ntohs(eth_type)) {
415 case ETH_P_IP:
416 tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
417 break;
418 case ETH_P_IPV6:
419 tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
420 break;
421 case ETH_P_1588:
422 tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
423 break;
424 default:
425 return;
426 }
427 tmp_rx_time.ident.seq_id =
428 ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
429 memcpy(tmp_rx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
430 PTP_SOURCE_PORT_LENGTH);
431 /* store rx timestamp */
432 tmp_rx_time.ts.sec = priv->prtc;
433 tmp_rx_time.ts.nsec = bdp_ex->ts;
434
435 /* insert timestamp in circular buffer */
436 fec_ptp_insert(&(priv->rx_timestamps), &tmp_rx_time);
437 }
438}
439
440
441static void fec_handle_ptpdrift(struct fec_enet_private *priv,
442 struct ptp_set_comp *comp, struct ptp_time_correct *ptc)
443{
444 u32 ndrift;
445 u32 i, adj_inc, adj_period;
446 u32 tmp_current, tmp_winner;
447 u32 ptp_ts_clk, ptp_inc;
448
449 ptp_ts_clk = clk_get_rate(priv->clk_ptp);
450 ptp_inc = FEC_T_PERIOD_ONE_SEC / ptp_ts_clk;
451
452 ndrift = comp->drift;
453
454 if (ndrift == 0) {
455 ptc->corr_inc = 0;
456 ptc->corr_period = 0;
457 return;
458 } else if (ndrift >= ptp_ts_clk) {
459 ptc->corr_inc = (u32)(ndrift / ptp_ts_clk);
460 ptc->corr_period = 1;
461 return;
462 } else {
463 tmp_winner = 0xFFFFFFFF;
464 adj_inc = 1;
465
466 if (ndrift > (ptp_ts_clk / ptp_inc)) {
467 adj_inc = ptp_inc / FEC_PTP_SPINNER_2;
468 } else if (ndrift > (ptp_ts_clk /
469 (ptp_inc * FEC_PTP_SPINNER_4))) {
470 adj_inc = ptp_inc / FEC_PTP_SPINNER_4;
471 adj_period = FEC_PTP_SPINNER_2;
472 } else {
473 adj_inc = FEC_PTP_SPINNER_4;
474 adj_period = FEC_PTP_SPINNER_4;
475 }
476
477 for (i = 1; i < adj_inc; i++) {
478 tmp_current = (ptp_ts_clk * i) % ndrift;
479 if (tmp_current == 0) {
480 ptc->corr_inc = i;
481 ptc->corr_period = (u32)((ptp_ts_clk *
482 adj_period * i) / ndrift);
483 break;
484 } else if (tmp_current < tmp_winner) {
485 ptc->corr_inc = i;
486 ptc->corr_period = (u32)((ptp_ts_clk *
487 adj_period * i) / ndrift);
488 tmp_winner = tmp_current;
489 }
490 }
491 }
492}
493
494static void fec_set_drift(struct fec_enet_private *priv,
495 struct ptp_set_comp *comp)
496{
497 struct ptp_time_correct tc;
498 u32 tmp, corr_ns;
499 u32 ptp_inc;
500
501 memset(&tc, 0, sizeof(struct ptp_time_correct));
502 fec_handle_ptpdrift(priv, comp, &tc);
503 if (tc.corr_inc == 0)
504 return;
505
506 ptp_inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(priv->clk_ptp);
507 if (comp->o_ops == TRUE)
508 corr_ns = ptp_inc + tc.corr_inc;
509 else
510 corr_ns = ptp_inc - tc.corr_inc;
511
512 tmp = readl(priv->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK;
513 tmp |= corr_ns << FEC_T_INC_CORR_OFFSET;
514 writel(tmp, priv->hwp + FEC_ATIME_INC);
515 writel(tc.corr_period, priv->hwp + FEC_ATIME_CORR);
516}
517
74/** 518/**
75 * fec_ptp_read - read raw cycle counter (to be used by time counter) 519 * fec_ptp_read - read raw cycle counter (to be used by time counter)
76 * @cc: the cyclecounter structure 520 * @cc: the cyclecounter structure
@@ -106,18 +550,25 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev)
106 unsigned long flags; 550 unsigned long flags;
107 int inc; 551 int inc;
108 552
109 inc = 1000000000 / fep->cycle_speed; 553 inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(fep->clk_ptp);
110 554
111 /* grab the ptp lock */ 555 /* grab the ptp lock */
112 spin_lock_irqsave(&fep->tmreg_lock, flags); 556 spin_lock_irqsave(&fep->tmreg_lock, flags);
557 writel(FEC_T_CTRL_RESTART, fep->hwp + FEC_ATIME_CTRL);
113 558
114 /* 1ns counter */ 559 /* 1ns counter */
115 writel(inc << FEC_T_INC_OFFSET, fep->hwp + FEC_ATIME_INC); 560 writel(inc << FEC_T_INC_OFFSET, fep->hwp + FEC_ATIME_INC);
116 561
117 /* use free running count */ 562 if (fep->hwts_rx_en_ioctl || fep->hwts_tx_en_ioctl) {
118 writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD); 563 writel(FEC_T_PERIOD_ONE_SEC, fep->hwp + FEC_ATIME_EVT_PERIOD);
119 564 /* start counter */
120 writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL); 565 writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE,
566 fep->hwp + FEC_ATIME_CTRL);
567 } else if (fep->hwts_tx_en || fep->hwts_tx_en) {
568 /* use free running count */
569 writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD);
570 writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL);
571 }
121 572
122 memset(&fep->cc, 0, sizeof(fep->cc)); 573 memset(&fep->cc, 0, sizeof(fep->cc));
123 fep->cc.read = fec_ptp_read; 574 fep->cc.read = fec_ptp_read;
@@ -277,50 +728,140 @@ static int fec_ptp_enable(struct ptp_clock_info *ptp,
277int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd) 728int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
278{ 729{
279 struct fec_enet_private *fep = netdev_priv(ndev); 730 struct fec_enet_private *fep = netdev_priv(ndev);
280
281 struct hwtstamp_config config; 731 struct hwtstamp_config config;
282 732 struct ptp_rtc_time curr_time;
283 if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 733 struct ptp_time rx_time, tx_time;
284 return -EFAULT; 734 struct fec_ptp_ts_data p_ts;
285 735 struct fec_ptp_ts_data *p_ts_user;
286 /* reserved for future extensions */ 736 struct ptp_set_comp p_comp;
287 if (config.flags) 737 u32 freq_compensation;
288 return -EINVAL; 738 int retval = 0;
289 739
290 switch (config.tx_type) { 740 switch (cmd) {
291 case HWTSTAMP_TX_OFF: 741 case SIOCSHWTSTAMP:
742 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
743 return -EFAULT;
744
745 /* reserved for future extensions */
746 if (config.flags)
747 return -EINVAL;
748
749 switch (config.tx_type) {
750 case HWTSTAMP_TX_OFF:
751 fep->hwts_tx_en = 0;
752 break;
753 case HWTSTAMP_TX_ON:
754 fep->hwts_tx_en = 1;
755 fep->hwts_tx_en_ioctl = 0;
756 break;
757 default:
758 return -ERANGE;
759 }
760
761 switch (config.rx_filter) {
762 case HWTSTAMP_FILTER_NONE:
763 if (fep->hwts_rx_en)
764 fep->hwts_rx_en = 0;
765 config.rx_filter = HWTSTAMP_FILTER_NONE;
766 break;
767
768 default:
769 /*
770 * register RXMTRL must be set in order
771 * to do V1 packets, therefore it is not
772 * possible to time stamp both V1 Sync and
773 * Delay_Req messages and hardware does not support
774 * timestamping all packets => return error
775 */
776 fep->hwts_rx_en = 1;
777 fep->hwts_rx_en_ioctl = 0;
778 config.rx_filter = HWTSTAMP_FILTER_ALL;
779 break;
780 }
781
782 fec_ptp_start_cyclecounter(ndev);
783 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
784 -EFAULT : 0;
785 break;
786 case PTP_ENBL_TXTS_IOCTL:
787 case PTP_ENBL_RXTS_IOCTL:
788 fep->hwts_rx_en_ioctl = 1;
789 fep->hwts_tx_en_ioctl = 1;
790 fep->hwts_rx_en = 0;
292 fep->hwts_tx_en = 0; 791 fep->hwts_tx_en = 0;
792 fec_ptp_start_cyclecounter(ndev);
293 break; 793 break;
294 case HWTSTAMP_TX_ON: 794 case PTP_DSBL_RXTS_IOCTL:
295 fep->hwts_tx_en = 1; 795 case PTP_DSBL_TXTS_IOCTL:
796 fep->hwts_rx_en_ioctl = 0;
797 fep->hwts_tx_en_ioctl = 0;
296 break; 798 break;
297 default: 799 case PTP_GET_RX_TIMESTAMP:
298 return -ERANGE; 800 p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
299 } 801 if (0 != copy_from_user(&p_ts.ident,
300 802 &p_ts_user->ident, sizeof(p_ts.ident)))
301 switch (config.rx_filter) { 803 return -EINVAL;
302 case HWTSTAMP_FILTER_NONE: 804 retval = fec_ptp_find_and_remove(&fep->rx_timestamps,
303 if (fep->hwts_rx_en) 805 &p_ts.ident, &rx_time);
304 fep->hwts_rx_en = 0; 806 if (retval == 0 &&
305 config.rx_filter = HWTSTAMP_FILTER_NONE; 807 copy_to_user((void __user *)(&p_ts_user->ts),
808 &rx_time, sizeof(rx_time)))
809 return -EFAULT;
306 break; 810 break;
307 811 case PTP_GET_TX_TIMESTAMP:
308 default: 812 p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
309 /* 813 if (0 != copy_from_user(&p_ts.ident,
310 * register RXMTRL must be set in order to do V1 packets, 814 &p_ts_user->ident, sizeof(p_ts.ident)))
311 * therefore it is not possible to time stamp both V1 Sync and 815 return -EINVAL;
312 * Delay_Req messages and hardware does not support 816 retval = fec_ptp_find_and_remove(&fep->tx_timestamps,
313 * timestamping all packets => return error 817 &p_ts.ident, &tx_time);
314 */ 818 if (retval == 0 &&
315 fep->hwts_rx_en = 1; 819 copy_to_user((void __user *)(&p_ts_user->ts),
316 config.rx_filter = HWTSTAMP_FILTER_ALL; 820 &tx_time, sizeof(tx_time)))
821 retval = -EFAULT;
822 break;
823 case PTP_GET_CURRENT_TIME:
824 fec_get_curr_cnt(fep, &curr_time);
825 if (0 != copy_to_user(ifr->ifr_data,
826 &(curr_time.rtc_time),
827 sizeof(struct ptp_time)))
828 return -EFAULT;
829 break;
830 case PTP_SET_RTC_TIME:
831 if (0 != copy_from_user(&(curr_time.rtc_time),
832 ifr->ifr_data,
833 sizeof(struct ptp_time)))
834 return -EINVAL;
835 fec_set_1588cnt(fep, &curr_time);
317 break; 836 break;
837 case PTP_FLUSH_TIMESTAMP:
838 /* reset tx-timestamping buffer */
839 fep->tx_timestamps.front = 0;
840 fep->tx_timestamps.end = 0;
841 fep->tx_timestamps.size = (DEFAULT_PTP_TX_BUF_SZ + 1);
842 /* reset rx-timestamping buffer */
843 fep->rx_timestamps.front = 0;
844 fep->rx_timestamps.end = 0;
845 fep->rx_timestamps.size = (DEFAULT_PTP_RX_BUF_SZ + 1);
846 break;
847 case PTP_SET_COMPENSATION:
848 if (0 != copy_from_user(&p_comp, ifr->ifr_data,
849 sizeof(struct ptp_set_comp)))
850 return -EINVAL;
851 fec_set_drift(fep, &p_comp);
852 break;
853 case PTP_GET_ORIG_COMP:
854 freq_compensation = FEC_PTP_ORIG_COMP;
855 if (copy_to_user(ifr->ifr_data, &freq_compensation,
856 sizeof(freq_compensation)) > 0)
857 return -EFAULT;
858 break;
859 default:
860 return -EINVAL;
318 } 861 }
319 862
320 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? 863 return retval;
321 -EFAULT : 0;
322} 864}
323
324/** 865/**
325 * fec_time_keep - call timecounter_read every second to avoid timer overrun 866 * fec_time_keep - call timecounter_read every second to avoid timer overrun
326 * because ENET just support 32bit counter, will timeout in 4s 867 * because ENET just support 32bit counter, will timeout in 4s
@@ -383,4 +924,22 @@ void fec_ptp_init(struct platform_device *pdev)
383 fep->ptp_clock = NULL; 924 fep->ptp_clock = NULL;
384 pr_err("ptp_clock_register failed\n"); 925 pr_err("ptp_clock_register failed\n");
385 } 926 }
927
928 /* initialize circular buffer for tx timestamps */
929 if (fec_ptp_init_circ(&(fep->tx_timestamps),
930 (DEFAULT_PTP_TX_BUF_SZ+1)))
931 pr_err("init tx circular buffer failed\n");
932 /* initialize circular buffer for rx timestamps */
933 if (fec_ptp_init_circ(&(fep->rx_timestamps),
934 (DEFAULT_PTP_RX_BUF_SZ+1)))
935 pr_err("init rx curcular buffer failed\n");
386} 936}
937
938void fec_ptp_cleanup(struct fec_enet_private *priv)
939{
940 if (priv->tx_timestamps.data_buf)
941 vfree(priv->tx_timestamps.data_buf);
942 if (priv->rx_timestamps.data_buf)
943 vfree(priv->rx_timestamps.data_buf);
944}
945
generated by cgit v1.2.2 (git 2.25.0) at 2025-01-09 18:09:05 -0500