1
2
3
4
5
6
7
8
9
10
11
12#include <linux/io.h>
13#include <linux/delay.h>
14#include "common.h"
15#include "stmmac_ptp.h"
16
17static void config_hw_tstamping(void __iomem *ioaddr, u32 data)
18{
19 writel(data, ioaddr + PTP_TCR);
20}
21
22static void config_sub_second_increment(void __iomem *ioaddr,
23 u32 ptp_clock, int gmac4, u32 *ssinc)
24{
25 u32 value = readl(ioaddr + PTP_TCR);
26 unsigned long data;
27 u32 reg_value;
28
29
30
31
32
33 if (value & PTP_TCR_TSCFUPDT)
34 data = (1000000000ULL / 50000000);
35 else
36 data = (1000000000ULL / ptp_clock);
37
38
39 if (!(value & PTP_TCR_TSCTRLSSR))
40 data = (data * 1000) / 465;
41
42 data &= PTP_SSIR_SSINC_MASK;
43
44 reg_value = data;
45 if (gmac4)
46 reg_value <<= GMAC4_PTP_SSIR_SSINC_SHIFT;
47
48 writel(reg_value, ioaddr + PTP_SSIR);
49
50 if (ssinc)
51 *ssinc = data;
52}
53
54static int init_systime(void __iomem *ioaddr, u32 sec, u32 nsec)
55{
56 int limit;
57 u32 value;
58
59 writel(sec, ioaddr + PTP_STSUR);
60 writel(nsec, ioaddr + PTP_STNSUR);
61
62 value = readl(ioaddr + PTP_TCR);
63 value |= PTP_TCR_TSINIT;
64 writel(value, ioaddr + PTP_TCR);
65
66
67 limit = 10;
68 while (limit--) {
69 if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSINIT))
70 break;
71 mdelay(10);
72 }
73 if (limit < 0)
74 return -EBUSY;
75
76 return 0;
77}
78
79static int config_addend(void __iomem *ioaddr, u32 addend)
80{
81 u32 value;
82 int limit;
83
84 writel(addend, ioaddr + PTP_TAR);
85
86 value = readl(ioaddr + PTP_TCR);
87 value |= PTP_TCR_TSADDREG;
88 writel(value, ioaddr + PTP_TCR);
89
90
91 limit = 10;
92 while (limit--) {
93 if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSADDREG))
94 break;
95 mdelay(10);
96 }
97 if (limit < 0)
98 return -EBUSY;
99
100 return 0;
101}
102
103static int adjust_systime(void __iomem *ioaddr, u32 sec, u32 nsec,
104 int add_sub, int gmac4)
105{
106 u32 value;
107 int limit;
108
109 if (add_sub) {
110
111
112
113
114 if (gmac4)
115 sec = -sec;
116
117 value = readl(ioaddr + PTP_TCR);
118 if (value & PTP_TCR_TSCTRLSSR)
119 nsec = (PTP_DIGITAL_ROLLOVER_MODE - nsec);
120 else
121 nsec = (PTP_BINARY_ROLLOVER_MODE - nsec);
122 }
123
124 writel(sec, ioaddr + PTP_STSUR);
125 value = (add_sub << PTP_STNSUR_ADDSUB_SHIFT) | nsec;
126 writel(value, ioaddr + PTP_STNSUR);
127
128
129 value = readl(ioaddr + PTP_TCR);
130 value |= PTP_TCR_TSUPDT;
131 writel(value, ioaddr + PTP_TCR);
132
133
134 limit = 10;
135 while (limit--) {
136 if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSUPDT))
137 break;
138 mdelay(10);
139 }
140 if (limit < 0)
141 return -EBUSY;
142
143 return 0;
144}
145
146static void get_systime(void __iomem *ioaddr, u64 *systime)
147{
148 u64 ns;
149
150
151 ns = readl(ioaddr + PTP_STNSR);
152
153 ns += readl(ioaddr + PTP_STSR) * 1000000000ULL;
154
155 if (systime)
156 *systime = ns;
157}
158
159const struct stmmac_hwtimestamp stmmac_ptp = {
160 .config_hw_tstamping = config_hw_tstamping,
161 .init_systime = init_systime,
162 .config_sub_second_increment = config_sub_second_increment,
163 .config_addend = config_addend,
164 .adjust_systime = adjust_systime,
165 .get_systime = get_systime,
166};
167