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