1 /*
2 * Copyright (c) 2024, The OpenThread Authors.
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * 1. Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * 2. Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * 3. Neither the name of the copyright holder nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
20 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 * POSSIBILITY OF SUCH DAMAGE.
27 */
28
29 #include "simul_utils.h"
30
31 #include <errno.h>
32 #include <ifaddrs.h>
33 #include <net/if.h>
34 #include <sys/time.h>
35
36 #include "utils/code_utils.h"
37
38 #define UTILS_SOCKET_LOCAL_HOST_ADDR "127.0.0.1"
39 #define UTILS_SOCKET_GROUP_ADDR "224.0.0.116"
40 #define UTILS_SOCKET_GROUP_ADDR6 "ff02::116"
41
42 const char *gLocalInterface = UTILS_SOCKET_LOCAL_HOST_ADDR;
43
utilsAddFdToFdSet(int aFd,fd_set * aFdSet,int * aMaxFd)44 void utilsAddFdToFdSet(int aFd, fd_set *aFdSet, int *aMaxFd)
45 {
46 otEXPECT(aFd >= 0);
47 otEXPECT(aFdSet != NULL);
48
49 FD_SET(aFd, aFdSet);
50
51 otEXPECT(aMaxFd != NULL);
52
53 if (*aMaxFd < aFd)
54 {
55 *aMaxFd = aFd;
56 }
57
58 exit:
59 return;
60 }
61
IsAddressLinkLocal(const struct in6_addr * aAddress)62 static bool IsAddressLinkLocal(const struct in6_addr *aAddress)
63 {
64 return ((aAddress->s6_addr[0] & 0xff) == 0xfe) && ((aAddress->s6_addr[1] & 0xc0) == 0x80);
65 }
66
InitRxSocket(utilsSocket * aSocket,const struct in_addr * aIp4Address,unsigned int aIfIndex)67 static void InitRxSocket(utilsSocket *aSocket, const struct in_addr *aIp4Address, unsigned int aIfIndex)
68 {
69 int fd;
70 int one = 1;
71 int rval;
72
73 fd = socket(aIp4Address ? AF_INET : AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
74 otEXPECT_ACTION(fd != -1, perror("socket(RxFd)"));
75
76 rval = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
77 otEXPECT_ACTION(rval != -1, perror("setsockopt(RxFd, SO_REUSEADDR)"));
78
79 rval = setsockopt(fd, SOL_SOCKET, SO_REUSEPORT, &one, sizeof(one));
80 otEXPECT_ACTION(rval != -1, perror("setsockopt(RxFd, SO_REUSEPORT)"));
81
82 if (aIp4Address)
83 {
84 struct ip_mreqn mreq;
85 struct sockaddr_in *sockaddr = &aSocket->mGroupAddr.mSockAddr4;
86
87 rval = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, aIp4Address, sizeof(*aIp4Address));
88 otEXPECT_ACTION(rval != -1, perror("setsockopt(RxFd, IP_MULTICAST_IF)"));
89
90 memset(sockaddr, 0, sizeof(*sockaddr));
91 sockaddr->sin_family = AF_INET;
92 sockaddr->sin_port = htons(aSocket->mPortBase);
93 otEXPECT_ACTION(inet_pton(AF_INET, UTILS_SOCKET_GROUP_ADDR, &sockaddr->sin_addr), perror("inet_pton(AF_INET)"));
94
95 memset(&mreq, 0, sizeof(mreq));
96 mreq.imr_multiaddr = sockaddr->sin_addr;
97 mreq.imr_address = *aIp4Address; // This address is used to identify the network interface
98
99 rval = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
100 otEXPECT_ACTION(rval != -1, perror("setsockopt(RxFd, IP_ADD_MEMBERSHIP)"));
101
102 rval = bind(fd, (struct sockaddr *)sockaddr, sizeof(*sockaddr));
103 otEXPECT_ACTION(rval != -1, perror("bind(RxFd)"));
104 }
105 else
106 {
107 struct ipv6_mreq mreq;
108 struct sockaddr_in6 *sockaddr = &aSocket->mGroupAddr.mSockAddr6;
109
110 rval = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_IF, &aIfIndex, sizeof(aIfIndex));
111 otEXPECT_ACTION(rval != -1, perror("setsockopt(RxFd, IPV6_MULTICAST_IF)"));
112
113 memset(sockaddr, 0, sizeof(*sockaddr));
114 sockaddr->sin6_family = AF_INET6;
115 sockaddr->sin6_port = htons(aSocket->mPortBase);
116 sockaddr->sin6_scope_id = aIfIndex; // This specifies network interface for link local scope
117 otEXPECT_ACTION(inet_pton(AF_INET6, UTILS_SOCKET_GROUP_ADDR6, &sockaddr->sin6_addr),
118 perror("inet_pton(AF_INET6)"));
119
120 memset(&mreq, 0, sizeof(mreq));
121 mreq.ipv6mr_multiaddr = sockaddr->sin6_addr;
122 mreq.ipv6mr_interface = aIfIndex;
123
124 rval = setsockopt(fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq));
125 otEXPECT_ACTION(rval != -1, perror("setsockopt(RxFd, IPV6_JOIN_GROUP)"));
126
127 rval = bind(fd, (struct sockaddr *)sockaddr, sizeof(*sockaddr));
128 otEXPECT_ACTION(rval != -1, perror("bind(RxFd)"));
129 }
130
131 aSocket->mRxFd = fd;
132
133 exit:
134 if (aSocket->mRxFd == -1)
135 {
136 exit(EXIT_FAILURE);
137 }
138 }
139
InitTxSocketIp6(utilsSocket * aSocket,const struct in6_addr * aAddress,unsigned int aIfIndex)140 void InitTxSocketIp6(utilsSocket *aSocket, const struct in6_addr *aAddress, unsigned int aIfIndex)
141 {
142 int fd;
143 int one = 1;
144 int rval;
145 struct sockaddr_in6 sockaddr;
146
147 fd = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
148 otEXPECT_ACTION(fd != -1, perror("socket(TxFd)"));
149
150 memset(&sockaddr, 0, sizeof(sockaddr));
151 sockaddr.sin6_family = AF_INET6;
152 sockaddr.sin6_addr = *aAddress;
153 sockaddr.sin6_port = htons(aSocket->mPort);
154 if (IsAddressLinkLocal(aAddress))
155 {
156 sockaddr.sin6_scope_id = aIfIndex;
157 }
158
159 rval = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_IF, &aIfIndex, sizeof(aIfIndex));
160 otEXPECT_ACTION(rval != -1, perror("setsockopt(TxFd, IPV6_MULTICAST_IF)"));
161
162 rval = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &one, sizeof(one));
163 otEXPECT_ACTION(rval != -1, perror("setsockopt(TxFd, IPV6_MULTICAST_LOOP)"));
164
165 rval = bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
166 otEXPECT_ACTION(rval != -1, perror("bind(TxFd)"));
167
168 aSocket->mTxFd = fd;
169
170 exit:
171 if (aSocket->mTxFd == -1)
172 {
173 exit(EXIT_FAILURE);
174 }
175 }
176
InitTxSocketIp4(utilsSocket * aSocket,const struct in_addr * aAddress)177 static void InitTxSocketIp4(utilsSocket *aSocket, const struct in_addr *aAddress)
178 {
179 int fd;
180 int one = 1;
181 int rval;
182 struct sockaddr_in sockaddr;
183
184 //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
185 // Prepare `mTxFd`
186
187 fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
188 otEXPECT_ACTION(fd != -1, perror("socket(TxFd)"));
189
190 memset(&sockaddr, 0, sizeof(sockaddr));
191 sockaddr.sin_family = AF_INET;
192 sockaddr.sin_port = htons(aSocket->mPort);
193 sockaddr.sin_addr = *aAddress;
194
195 rval = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &sockaddr.sin_addr, sizeof(sockaddr.sin_addr));
196 otEXPECT_ACTION(rval != -1, perror("setsockopt(TxFd, IP_MULTICAST_IF)"));
197
198 rval = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &one, sizeof(one));
199 otEXPECT_ACTION(rval != -1, perror("setsockopt(TxFd, IP_MULTICAST_LOOP)"));
200
201 rval = bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
202 otEXPECT_ACTION(rval != -1, perror("bind(TxFd)"));
203
204 aSocket->mTxFd = fd;
205
206 exit:
207 if (aSocket->mTxFd == -1)
208 {
209 exit(EXIT_FAILURE);
210 }
211 }
212
TryInitSocketIfname(utilsSocket * aSocket,const char * aLocalInterface)213 static bool TryInitSocketIfname(utilsSocket *aSocket, const char *aLocalInterface)
214 {
215 const struct in6_addr *addr6 = NULL;
216 const struct in6_addr *addr6ll = NULL;
217 const struct in_addr *addr4 = NULL;
218 struct ifaddrs *ifaddr = NULL;
219 unsigned int ifIndex = 0;
220
221 otEXPECT((ifIndex = if_nametoindex(aLocalInterface)));
222
223 if (getifaddrs(&ifaddr) == -1)
224 {
225 perror("getifaddrs");
226 exit(EXIT_FAILURE);
227 }
228
229 for (struct ifaddrs *ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next)
230 {
231 if (ifa->ifa_addr == NULL || strcmp(ifa->ifa_name, aLocalInterface) != 0)
232 {
233 continue;
234 }
235
236 if (ifa->ifa_addr->sa_family == AF_INET)
237 {
238 addr4 = &((const struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
239 }
240 else if (ifa->ifa_addr->sa_family == AF_INET6)
241 {
242 addr6 = &((const struct sockaddr_in6 *)ifa->ifa_addr)->sin6_addr;
243 if (IsAddressLinkLocal(addr6))
244 {
245 addr6ll = addr6;
246 }
247 }
248 }
249
250 // Prefer
251 // 1. IPv6 link local address
252 // 2. IPv4 addresses
253 // 3. IPv6 addresses
254 if (addr6ll)
255 {
256 InitTxSocketIp6(aSocket, addr6ll, ifIndex);
257 addr6 = addr6ll;
258 }
259 else if (addr4)
260 {
261 InitTxSocketIp4(aSocket, addr4);
262 addr6 = NULL;
263 }
264 else if (addr6)
265 {
266 InitTxSocketIp6(aSocket, addr6, ifIndex);
267 }
268 else
269 {
270 fprintf(stderr, "No sock address for TX socket!\n");
271 exit(EXIT_FAILURE);
272 }
273
274 InitRxSocket(aSocket, (addr6 ? NULL : addr4), ifIndex);
275 aSocket->mInitialized = true;
276 aSocket->mUseIp6 = (addr6 != NULL);
277
278 exit:
279 freeifaddrs(ifaddr);
280 return aSocket->mInitialized;
281 }
282
TryInitSocketIp4(utilsSocket * aSocket,const char * aLocalInterface)283 static bool TryInitSocketIp4(utilsSocket *aSocket, const char *aLocalInterface)
284 {
285 struct in_addr addr4;
286
287 otEXPECT(inet_pton(AF_INET, aLocalInterface, &addr4));
288
289 InitTxSocketIp4(aSocket, &addr4);
290 InitRxSocket(aSocket, &addr4, 0);
291 aSocket->mInitialized = true;
292 aSocket->mUseIp6 = false;
293
294 exit:
295 return aSocket->mInitialized;
296 }
297
TryInitSocketIp6(utilsSocket * aSocket,const char * aLocalInterface)298 static bool TryInitSocketIp6(utilsSocket *aSocket, const char *aLocalInterface)
299 {
300 struct in6_addr addr6;
301 struct ifaddrs *ifaddr = NULL;
302
303 otEXPECT(inet_pton(AF_INET6, aLocalInterface, &addr6));
304
305 if (getifaddrs(&ifaddr) == -1)
306 {
307 perror("getifaddrs");
308 exit(EXIT_FAILURE);
309 }
310
311 for (struct ifaddrs *ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next)
312 {
313 const struct sockaddr_in6 *sockaddr6;
314 unsigned int ifIndex;
315
316 if (ifa->ifa_addr == NULL || ifa->ifa_addr->sa_family != AF_INET6)
317 {
318 continue;
319 }
320
321 sockaddr6 = (const struct sockaddr_in6 *)ifa->ifa_addr;
322 if (memcmp(&sockaddr6->sin6_addr, &addr6, sizeof(addr6)))
323 {
324 continue;
325 }
326
327 ifIndex = if_nametoindex(ifa->ifa_name);
328 if (ifIndex == 0)
329 {
330 perror("if_nametoindex");
331 exit(EXIT_FAILURE);
332 }
333
334 InitTxSocketIp6(aSocket, &addr6, ifIndex);
335 InitRxSocket(aSocket, NULL, ifIndex);
336 aSocket->mInitialized = true;
337 aSocket->mUseIp6 = true;
338 break;
339 }
340
341 exit:
342 freeifaddrs(ifaddr);
343 return aSocket->mInitialized;
344 }
345
utilsInitSocket(utilsSocket * aSocket,uint16_t aPortBase)346 void utilsInitSocket(utilsSocket *aSocket, uint16_t aPortBase)
347 {
348 aSocket->mInitialized = false;
349 aSocket->mPortBase = aPortBase;
350 aSocket->mTxFd = -1;
351 aSocket->mRxFd = -1;
352 aSocket->mPort = (uint16_t)(aSocket->mPortBase + gNodeId);
353
354 if (!TryInitSocketIfname(aSocket, gLocalInterface) && !TryInitSocketIp4(aSocket, gLocalInterface) &&
355 !TryInitSocketIp6(aSocket, gLocalInterface))
356 {
357 fprintf(stderr, "Failed to simulate node %d on %s\n", gNodeId, gLocalInterface);
358 exit(EXIT_FAILURE);
359 }
360 }
361
utilsDeinitSocket(utilsSocket * aSocket)362 void utilsDeinitSocket(utilsSocket *aSocket)
363 {
364 if (aSocket->mInitialized)
365 {
366 close(aSocket->mRxFd);
367 close(aSocket->mTxFd);
368 aSocket->mInitialized = false;
369 }
370 }
371
utilsAddSocketRxFd(const utilsSocket * aSocket,fd_set * aFdSet,int * aMaxFd)372 void utilsAddSocketRxFd(const utilsSocket *aSocket, fd_set *aFdSet, int *aMaxFd)
373 {
374 otEXPECT(aSocket->mInitialized);
375 utilsAddFdToFdSet(aSocket->mRxFd, aFdSet, aMaxFd);
376
377 exit:
378 return;
379 }
380
utilsAddSocketTxFd(const utilsSocket * aSocket,fd_set * aFdSet,int * aMaxFd)381 void utilsAddSocketTxFd(const utilsSocket *aSocket, fd_set *aFdSet, int *aMaxFd)
382 {
383 otEXPECT(aSocket->mInitialized);
384 utilsAddFdToFdSet(aSocket->mTxFd, aFdSet, aMaxFd);
385
386 exit:
387 return;
388 }
389
utilsCanSocketReceive(const utilsSocket * aSocket,const fd_set * aReadFdSet)390 bool utilsCanSocketReceive(const utilsSocket *aSocket, const fd_set *aReadFdSet)
391 {
392 return aSocket->mInitialized && FD_ISSET(aSocket->mRxFd, aReadFdSet);
393 }
394
utilsCanSocketSend(const utilsSocket * aSocket,const fd_set * aWriteFdSet)395 bool utilsCanSocketSend(const utilsSocket *aSocket, const fd_set *aWriteFdSet)
396 {
397 return aSocket->mInitialized && FD_ISSET(aSocket->mTxFd, aWriteFdSet);
398 }
399
utilsReceiveFromSocket(const utilsSocket * aSocket,void * aBuffer,uint16_t aBufferSize,uint16_t * aSenderNodeId)400 uint16_t utilsReceiveFromSocket(const utilsSocket *aSocket,
401 void *aBuffer,
402 uint16_t aBufferSize,
403 uint16_t *aSenderNodeId)
404 {
405 ssize_t rval;
406 uint16_t len = 0;
407 union
408 {
409 struct sockaddr_in sockaddr4;
410 struct sockaddr_in6 sockaddr6;
411 } sockaddr;
412 socklen_t socklen = aSocket->mUseIp6 ? sizeof(sockaddr.sockaddr6) : sizeof(sockaddr.sockaddr4);
413
414 memset(&sockaddr, 0, sizeof(sockaddr));
415
416 rval = recvfrom(aSocket->mRxFd, (char *)aBuffer, aBufferSize, 0, (struct sockaddr *)&sockaddr, &socklen);
417
418 if (rval > 0)
419 {
420 uint16_t senderPort = ntohs(aSocket->mUseIp6 ? sockaddr.sockaddr6.sin6_port : sockaddr.sockaddr4.sin_port);
421
422 if (aSenderNodeId != NULL)
423 {
424 *aSenderNodeId = (uint16_t)(senderPort - aSocket->mPortBase);
425 }
426
427 len = (uint16_t)rval;
428 }
429 else if (rval == 0)
430 {
431 assert(false);
432 }
433 else if (errno != EINTR && errno != EAGAIN)
434 {
435 perror("recvfrom(RxFd)");
436 exit(EXIT_FAILURE);
437 }
438
439 return len;
440 }
441
utilsSendOverSocket(const utilsSocket * aSocket,const void * aBuffer,uint16_t aBufferLength)442 void utilsSendOverSocket(const utilsSocket *aSocket, const void *aBuffer, uint16_t aBufferLength)
443 {
444 ssize_t rval;
445
446 rval =
447 sendto(aSocket->mTxFd, (const char *)aBuffer, aBufferLength, 0, (const struct sockaddr *)&aSocket->mGroupAddr,
448 (aSocket->mUseIp6 ? sizeof(aSocket->mGroupAddr.mSockAddr6) : sizeof(aSocket->mGroupAddr.mSockAddr4)));
449
450 if (rval < 0)
451 {
452 perror("sendto(sTxFd)");
453 exit(EXIT_FAILURE);
454 }
455 }
456