1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
/*
* Copyright (c) 2006-2023, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date Author Notes
* 2023-11-25 GuEe-GUI first version
*/
#include "macb.h"
#define DBG_TAG "eth.macb.ptp"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>
#define NSEC_PER_SEC 1000000000L
#define GEM_PTP_MAX_FREQ 500000000
#define TSU_NSEC_MAX_VAL ((1 << 30) - 1)
rt_inline rt_uint32_t gem_readl(struct macb_eth *eth, rt_uint32_t off)
{
return *(volatile rt_uint32_t *)((rt_uint8_t *)eth->regs + off);
}
rt_inline void gem_writel(struct macb_eth *eth, rt_uint32_t off, rt_uint32_t val)
{
*(volatile rt_uint32_t *)((rt_uint8_t *)eth->regs + off) = val;
}
static rt_bool_t gem_tsu_present(struct macb_eth *eth)
{
rt_uint32_t dcfg5 = gem_readl(eth, GEM_DCFG5);
return (dcfg5 & (1u << GEM_DCFG5_TSU_SHIFT)) ? RT_TRUE : RT_FALSE;
}
static rt_err_t macb_ptp_adjfreq(struct rt_ptp_clock *ptp, rt_base_t freq)
{
rt_int64_t adj;
rt_uint32_t inc;
struct macb_eth *eth = raw_to_macb_ptp(ptp);
/* Base increment in GEM_TI (NSINCR field) */
inc = gem_readl(eth, GEM_TI) & 0xff;
if (inc == 0)
{
inc = 1;
}
adj = (rt_int64_t)inc * (rt_int64_t)freq / 1000000000LL;
if (adj > 0x7fffffff || adj < -0x80000000LL)
{
return -RT_EINVAL;
}
gem_writel(eth, GEM_TI, (inc + (rt_uint32_t)adj) & 0xff);
return RT_EOK;
}
static rt_err_t macb_ptp_adjtime(struct rt_ptp_clock *ptp, rt_int64_t delta)
{
rt_uint32_t adj;
struct macb_eth *eth = raw_to_macb_ptp(ptp);
if (delta < 0)
{
delta = -delta;
adj = RT_BIT(31) | (rt_uint32_t)(delta > TSU_NSEC_MAX_VAL ? TSU_NSEC_MAX_VAL : delta);
}
else
{
adj = (rt_uint32_t)(delta > TSU_NSEC_MAX_VAL ? TSU_NSEC_MAX_VAL : delta);
}
gem_writel(eth, GEM_TA, adj);
return RT_EOK;
}
static rt_err_t macb_ptp_gettime(struct rt_ptp_clock *ptp, struct rt_ptp_clock_time *ts)
{
rt_uint32_t tsh, tsl, tn;
struct macb_eth *eth = raw_to_macb_ptp(ptp);
tn = gem_readl(eth, GEM_TN);
tsl = gem_readl(eth, GEM_TSL);
tsh = gem_readl(eth, GEM_TSH) & 0xffffU;
ts->sec = ((rt_int64_t)tsh << 32) | tsl;
ts->nsec = (rt_int32_t)(tn & 0x3fffffffU);
return RT_EOK;
}
static rt_err_t macb_ptp_settime(struct rt_ptp_clock *ptp, const struct rt_ptp_clock_time *ts)
{
struct macb_eth *eth = raw_to_macb_ptp(ptp);
if (!ts || ts->nsec < 0 || ts->nsec >= NSEC_PER_SEC)
{
return -RT_EINVAL;
}
/* TN clear, TSH, TSL, then TN */
gem_writel(eth, GEM_TN, 0);
gem_writel(eth, GEM_TSH, (rt_uint32_t)((rt_uint64_t)ts->sec >> 32) & 0xffffU);
gem_writel(eth, GEM_TSL, (rt_uint32_t)ts->sec);
gem_writel(eth, GEM_TN, (rt_uint32_t)ts->nsec);
return RT_EOK;
}
static rt_err_t macb_ptp_enable(struct rt_ptp_clock *ptp, struct rt_ptp_clock_request *req, rt_bool_t on)
{
return RT_EOK;
}
static const struct rt_ptp_ops macb_gem_ptp_ops =
{
.adjfreq = macb_ptp_adjfreq,
.adjtime = macb_ptp_adjtime,
.gettime = macb_ptp_gettime,
.settime = macb_ptp_settime,
.enable = macb_ptp_enable,
};
rt_err_t macb_ptp_register(struct macb_eth *eth)
{
rt_err_t err;
struct rt_ptp_clock *ptp = ð->ptp_parent;
if (!gem_tsu_present(eth))
{
return -RT_ENOSYS;
}
ptp->ops = &macb_gem_ptp_ops;
ptp->max_freq = GEM_PTP_MAX_FREQ;
if ((err = rt_ptp_clock_register(ptp)))
{
ptp->ops = RT_NULL;
return err;
}
return RT_EOK;
}
void macb_ptp_unregister(struct macb_eth *eth)
{
struct rt_ptp_clock *ptp = ð->ptp_parent;
if (!ptp->ops)
{
return;
}
rt_ptp_clock_unregister(ptp);
ptp->ops = RT_NULL;
}
|
