1 /*
2 * Copyright (c) 2024 BayLibre SAS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(ptp_transport, CONFIG_PTP_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11
12 #include <zephyr/net/socket.h>
13
14 #include "transport.h"
15
16 #define INTERFACE_NAME_LEN (32)
17
18 #if CONFIG_PTP_UDP_IPv4_PROTOCOL
19 static struct in_addr mcast_addr = {{{224, 0, 1, 129}}};
20 #elif CONFIG_PTP_UDP_IPv6_PROTOCOL
21 static struct in6_addr mcast_addr = {{{0xff, 0xe, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
22 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x81}}};
23 #else
24 #error "Chosen PTP transport protocol not implemented"
25 #endif
26
transport_socket_open(struct net_if * iface,struct sockaddr * addr)27 static int transport_socket_open(struct net_if *iface, struct sockaddr *addr)
28 {
29 static const int feature_on = 1;
30 static const uint8_t priority = NET_PRIORITY_CA;
31 static const uint8_t ts_mask = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE;
32 struct ifreq ifreq = { 0 };
33 int cnt;
34 int socket = zsock_socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);
35
36 if (net_if_get_by_iface(iface) < 0) {
37 LOG_ERR("Failed to obtain interface index");
38 return -1;
39 }
40
41 if (socket < 0) {
42 return -1;
43 }
44
45 if (zsock_setsockopt(socket, SOL_SOCKET, SO_REUSEADDR, &feature_on, sizeof(feature_on))) {
46 LOG_ERR("Failed to set SO_REUSEADDR");
47 goto error;
48 }
49
50 if (zsock_bind(socket, addr, sizeof(*addr))) {
51 LOG_ERR("Failed to bind socket");
52 goto error;
53 }
54
55 cnt = net_if_get_name(iface, ifreq.ifr_name, INTERFACE_NAME_LEN);
56 if (cnt > 0 && zsock_setsockopt(socket,
57 SOL_SOCKET,
58 SO_BINDTODEVICE,
59 ifreq.ifr_name,
60 sizeof(ifreq.ifr_name))) {
61 LOG_ERR("Failed to set socket binding to an interface");
62 goto error;
63 }
64
65 if (zsock_setsockopt(socket, SOL_SOCKET, SO_TIMESTAMPING, &ts_mask, sizeof(ts_mask))) {
66 LOG_ERR("Failed to set SO_TIMESTAMPING");
67 goto error;
68 }
69
70 if (zsock_setsockopt(socket, SOL_SOCKET, SO_PRIORITY, &priority, sizeof(priority))) {
71 LOG_ERR("Failed to set SO_PRIORITY");
72 goto error;
73 }
74
75 return socket;
76 error:
77 zsock_close(socket);
78 return -1;
79 }
80
transport_join_multicast(struct ptp_port * port)81 static int transport_join_multicast(struct ptp_port *port)
82 {
83 if (IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL)) {
84 struct ip_mreqn mreqn = {0};
85
86 memcpy(&mreqn.imr_multiaddr, &mcast_addr, sizeof(struct in_addr));
87 mreqn.imr_ifindex = net_if_get_by_iface(port->iface);
88
89 zsock_setsockopt(port->socket[1], IPPROTO_IP,
90 IP_ADD_MEMBERSHIP, &mreqn, sizeof(mreqn));
91 } else {
92 struct ipv6_mreq mreqn = {0};
93
94 memcpy(&mreqn.ipv6mr_multiaddr, &mcast_addr, sizeof(struct in6_addr));
95 mreqn.ipv6mr_ifindex = net_if_get_by_iface(port->iface);
96
97 zsock_setsockopt(port->socket[0], IPPROTO_IPV6,
98 IPV6_ADD_MEMBERSHIP, &mreqn, sizeof(mreqn));
99 }
100
101 return 0;
102 }
103
transport_udp_ipv4_open(struct net_if * iface,uint16_t port)104 static int transport_udp_ipv4_open(struct net_if *iface, uint16_t port)
105 {
106 uint8_t tos;
107 socklen_t length;
108 int socket, ttl = 1;
109 struct sockaddr_in addr = {
110 .sin_family = AF_INET,
111 .sin_addr = INADDR_ANY_INIT,
112 .sin_port = htons(port),
113 };
114
115 socket = transport_socket_open(iface, (struct sockaddr *)&addr);
116 if (socket < 0) {
117 return -1;
118 }
119
120 if (zsock_setsockopt(socket, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl))) {
121 LOG_ERR("Failed to set ip multicast ttl socket option");
122 goto error;
123 }
124
125 if (zsock_getsockopt(socket, IPPROTO_IP, IP_TOS, &tos, &length)) {
126 tos = 0;
127 }
128
129 tos &= ~0xFC;
130 tos |= CONFIG_PTP_DSCP_VALUE << 2;
131 length = sizeof(tos);
132
133 if (zsock_setsockopt(socket, IPPROTO_IP, IP_TOS, &tos, length)) {
134 LOG_WRN("Failed to set DSCP priority");
135 }
136
137 return socket;
138 error:
139 zsock_close(socket);
140 return -1;
141 }
142
transport_udp_ipv6_open(struct net_if * iface,uint16_t port)143 static int transport_udp_ipv6_open(struct net_if *iface, uint16_t port)
144 {
145 uint8_t tclass;
146 socklen_t length;
147 int socket, hops = 1, feature_on = 1;
148 struct sockaddr_in6 addr = {
149 .sin6_family = AF_INET6,
150 .sin6_addr = IN6ADDR_ANY_INIT,
151 .sin6_port = htons(port)
152 };
153
154 socket = transport_socket_open(iface, (struct sockaddr *)&addr);
155 if (socket < 0) {
156 return -1;
157 }
158
159 if (zsock_setsockopt(socket,
160 IPPROTO_IPV6,
161 IPV6_RECVPKTINFO,
162 &feature_on,
163 sizeof(feature_on))) {
164 LOG_ERR("Failed to set IPV6_RECVPKTINFO");
165 goto error;
166 }
167
168 if (zsock_setsockopt(socket, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &hops, sizeof(hops))) {
169 LOG_ERR("Failed to set ip multicast hops socket option");
170 goto error;
171 }
172
173 if (zsock_getsockopt(socket, IPPROTO_IPV6, IPV6_TCLASS, &tclass, &length)) {
174 tclass = 0;
175 }
176
177 tclass &= ~0xFC;
178 tclass |= CONFIG_PTP_DSCP_VALUE << 2;
179 length = sizeof(tclass);
180
181 if (zsock_setsockopt(socket, IPPROTO_IPV6, IPV6_TCLASS, &tclass, length)) {
182 LOG_WRN("Failed to set priority");
183 }
184
185 return socket;
186 error:
187 zsock_close(socket);
188 return -1;
189 }
190
transport_send(int socket,int port,void * buf,int length,struct sockaddr * addr)191 static int transport_send(int socket, int port, void *buf, int length, struct sockaddr *addr)
192 {
193 struct sockaddr m_addr;
194 socklen_t addrlen;
195 int cnt;
196
197 if (!addr) {
198 if (IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL)) {
199 m_addr.sa_family = AF_INET;
200 net_sin(&m_addr)->sin_port = htons(port);
201 net_sin(&m_addr)->sin_addr.s_addr = mcast_addr.s_addr;
202
203 } else if (IS_ENABLED(CONFIG_PTP_UDP_IPv6_PROTOCOL)) {
204 m_addr.sa_family = AF_INET6;
205 net_sin6(&m_addr)->sin6_port = htons(port);
206 memcpy(&net_sin6(&m_addr)->sin6_addr,
207 &mcast_addr,
208 sizeof(struct in6_addr));
209 }
210 addr = &m_addr;
211 }
212
213 addrlen = IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL) ? sizeof(struct sockaddr_in) :
214 sizeof(struct sockaddr_in6);
215 cnt = zsock_sendto(socket, buf, length, 0, addr, addrlen);
216 if (cnt < 1) {
217 LOG_ERR("Failed to send message");
218 return -EFAULT;
219 }
220
221 return cnt;
222 }
223
ptp_transport_open(struct ptp_port * port)224 int ptp_transport_open(struct ptp_port *port)
225 {
226 static const int socket_ports[] = {PTP_SOCKET_PORT_EVENT, PTP_SOCKET_PORT_GENERAL};
227 int socket;
228
229 for (int i = 0; i < PTP_SOCKET_CNT; i++) {
230 socket = IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL) ?
231 transport_udp_ipv4_open(port->iface, socket_ports[i]) :
232 transport_udp_ipv6_open(port->iface, socket_ports[i]);
233
234 if (socket == -1) {
235 if (i == PTP_SOCKET_GENERAL) {
236 zsock_close(port->socket[PTP_SOCKET_EVENT]);
237 port->socket[PTP_SOCKET_EVENT] = -1;
238 }
239
240 return -1;
241 }
242
243 port->socket[i] = socket;
244 }
245
246 return transport_join_multicast(port);
247 }
248
ptp_transport_close(struct ptp_port * port)249 int ptp_transport_close(struct ptp_port *port)
250 {
251 for (int i = 0; i < PTP_SOCKET_CNT; i++) {
252
253 if (port->socket[i] >= 0) {
254 if (zsock_close(port->socket[i])) {
255 LOG_ERR("Failed to close socket on PTP Port %d",
256 port->port_ds.id.port_number);
257 return -1;
258 }
259 }
260
261 port->socket[i] = -1;
262 }
263
264 return 0;
265 }
266
ptp_transport_send(struct ptp_port * port,struct ptp_msg * msg,enum ptp_socket idx)267 int ptp_transport_send(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx)
268 {
269 __ASSERT(PTP_SOCKET_CNT <= idx, "Invalid socket index");
270
271 static const int socket_port[] = {PTP_SOCKET_PORT_EVENT, PTP_SOCKET_PORT_GENERAL};
272 int length = ntohs(msg->header.msg_length);
273
274 return transport_send(port->socket[idx], socket_port[idx], msg, length, NULL);
275 }
276
ptp_transport_sendto(struct ptp_port * port,struct ptp_msg * msg,enum ptp_socket idx)277 int ptp_transport_sendto(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx)
278 {
279 __ASSERT(PTP_SOCKET_CNT <= idx, "Invalid socket index");
280
281 static const int socket_port[] = {PTP_SOCKET_PORT_EVENT, PTP_SOCKET_PORT_GENERAL};
282 int length = ntohs(msg->header.msg_length);
283
284 return transport_send(port->socket[idx], socket_port[idx], msg, length, &msg->addr);
285 }
286
ptp_transport_recv(struct ptp_port * port,struct ptp_msg * msg,enum ptp_socket idx)287 int ptp_transport_recv(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx)
288 {
289 __ASSERT(PTP_SOCKET_CNT <= idx, "Invalid socket index");
290
291 int cnt = 0;
292 uint8_t ctrl[CMSG_SPACE(sizeof(struct net_ptp_time))] = {0};
293 struct cmsghdr *cmsg;
294 struct msghdr msghdr = {0};
295 struct iovec iov = {
296 .iov_base = msg,
297 .iov_len = sizeof(msg->mtu),
298 };
299
300 msghdr.msg_iov = &iov;
301 msghdr.msg_iovlen = 1;
302 msghdr.msg_control = ctrl;
303 msghdr.msg_controllen = sizeof(ctrl);
304
305 cnt = zsock_recvmsg(port->socket[idx], &msghdr, ZSOCK_MSG_DONTWAIT);
306 if (cnt < 0) {
307 LOG_ERR("Failed receive PTP message");
308 }
309
310 for (cmsg = CMSG_FIRSTHDR(&msghdr); cmsg != NULL; cmsg = CMSG_NXTHDR(&msghdr, cmsg)) {
311 if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING) {
312 memcpy(&msg->timestamp.host, CMSG_DATA(cmsg), sizeof(struct net_ptp_time));
313 }
314 }
315
316 return cnt;
317 }
318
ptp_transport_protocol_addr(struct ptp_port * port,uint8_t * addr)319 int ptp_transport_protocol_addr(struct ptp_port *port, uint8_t *addr)
320 {
321 __ASSERT_NO_MSG(addr);
322
323 int length = 0;
324
325 if (IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL)) {
326 struct in_addr *ip = net_if_ipv4_get_global_addr(port->iface, NET_ADDR_PREFERRED);
327
328 length = NET_IPV4_ADDR_SIZE;
329 *addr = ip->s_addr;
330 } else if (IS_ENABLED(CONFIG_PTP_UDP_IPv6_PROTOCOL)) {
331 struct in6_addr *ip = net_if_ipv6_get_global_addr(NET_ADDR_PREFERRED, &port->iface);
332
333 length = NET_IPV6_ADDR_SIZE;
334 memcpy(addr, ip, length);
335 }
336
337 return length;
338 }
339
ptp_transport_physical_addr(struct ptp_port * port)340 struct net_linkaddr *ptp_transport_physical_addr(struct ptp_port *port)
341 {
342 return net_if_get_link_addr(port->iface);
343 }
344
