standalone: fix w/h confusion
[pcsx_rearmed.git] / deps / libretro-common / compat / compat_ifaddrs.c
1 /*
2 Copyright (c) 2013, Kenneth MacKay
3 All rights reserved.
4
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright notice, this
8    list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright notice,
10    this list of conditions and the following disclaimer in the documentation
11    and/or other materials provided with the distribution.
12
13 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
14 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
16 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
17 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
18 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
21 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
22 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 */
24
25 #include <compat/ifaddrs.h>
26
27 #include <string.h>
28 #include <stdlib.h>
29 #include <stddef.h>
30 #include <errno.h>
31 #include <unistd.h>
32 #include <sys/socket.h>
33 #include <netpacket/packet.h>
34 #include <net/if_arp.h>
35 #include <netinet/in.h>
36 #include <linux/netlink.h>
37 #include <linux/rtnetlink.h>
38
39 typedef struct NetlinkList
40 {
41     struct NetlinkList *m_next;
42     struct nlmsghdr *m_data;
43     unsigned int m_size;
44 } NetlinkList;
45
46 static int netlink_socket(void)
47 {
48    struct sockaddr_nl l_addr;
49    int l_socket = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
50
51    if (l_socket < 0)
52       return -1;
53
54    memset(&l_addr, 0, sizeof(l_addr));
55    l_addr.nl_family = AF_NETLINK;
56
57    if (bind(l_socket, (struct sockaddr *)&l_addr, sizeof(l_addr)) < 0)
58    {
59       close(l_socket);
60       return -1;
61    }
62
63    return l_socket;
64 }
65
66 static int netlink_send(int p_socket, int p_request)
67 {
68    struct
69    {
70       struct nlmsghdr m_hdr;
71       struct rtgenmsg m_msg;
72    } l_data;
73    struct sockaddr_nl l_addr;
74
75    memset(&l_data, 0, sizeof(l_data));
76
77    l_data.m_hdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtgenmsg));
78    l_data.m_hdr.nlmsg_type = p_request;
79    l_data.m_hdr.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
80    l_data.m_hdr.nlmsg_pid = 0;
81    l_data.m_hdr.nlmsg_seq = p_socket;
82    l_data.m_msg.rtgen_family = AF_UNSPEC;
83
84    memset(&l_addr, 0, sizeof(l_addr));
85    l_addr.nl_family = AF_NETLINK;
86    return (sendto(p_socket, &l_data.m_hdr, l_data.m_hdr.nlmsg_len, 0, (struct sockaddr *)&l_addr, sizeof(l_addr)));
87 }
88
89 static int netlink_recv(int p_socket, void *p_buffer, size_t p_len)
90 {
91    struct msghdr l_msg;
92    struct iovec l_iov = { p_buffer, p_len };
93    struct sockaddr_nl l_addr;
94
95    for (;;)
96    {
97       int l_result;
98
99       l_msg.msg_name       = (void *)&l_addr;
100       l_msg.msg_namelen    = sizeof(l_addr);
101       l_msg.msg_iov        = &l_iov;
102       l_msg.msg_iovlen     = 1;
103       l_msg.msg_control    = NULL;
104       l_msg.msg_controllen = 0;
105       l_msg.msg_flags      = 0;
106
107       l_result             = recvmsg(p_socket, &l_msg, 0);
108
109       if (l_result < 0)
110       {
111          if (errno == EINTR)
112             continue;
113          return -2;
114       }
115
116       if (l_msg.msg_flags & MSG_TRUNC) /* buffer too small */
117          return -1;
118       return l_result;
119    }
120 }
121
122 static struct nlmsghdr *getNetlinkResponse(int p_socket,
123       int *p_size, int *p_done)
124 {
125    size_t l_size  = 4096;
126    void *l_buffer = NULL;
127
128    for (;;)
129    {
130       int l_read;
131
132       free(l_buffer);
133       l_buffer = malloc(l_size);
134       if (!l_buffer)
135          return NULL;
136
137       l_read  = netlink_recv(p_socket, l_buffer, l_size);
138       *p_size = l_read;
139
140       if (l_read == -2)
141       {
142          free(l_buffer);
143          return NULL;
144       }
145
146       if (l_read >= 0)
147       {
148          pid_t l_pid = getpid();
149          struct nlmsghdr *l_hdr;
150
151          for (l_hdr = (struct nlmsghdr *)l_buffer;
152                NLMSG_OK(l_hdr, (unsigned int)l_read);
153                l_hdr = (struct nlmsghdr *)NLMSG_NEXT(l_hdr, l_read))
154          {
155             if (  (pid_t)l_hdr->nlmsg_pid != l_pid || 
156                   (int)l_hdr->nlmsg_seq   != p_socket)
157                continue;
158
159             if (l_hdr->nlmsg_type == NLMSG_DONE)
160             {
161                *p_done = 1;
162                break;
163             }
164
165             if (l_hdr->nlmsg_type == NLMSG_ERROR)
166             {
167                free(l_buffer);
168                return NULL;
169             }
170          }
171          return l_buffer;
172       }
173
174       l_size *= 2;
175    }
176 }
177
178 static NetlinkList *newListItem(struct nlmsghdr *p_data, unsigned int p_size)
179 {
180    NetlinkList *l_item = (NetlinkList*)malloc(sizeof(NetlinkList));
181    if (!l_item)
182       return NULL;
183
184    l_item->m_next = NULL;
185    l_item->m_data = p_data;
186    l_item->m_size = p_size;
187    return l_item;
188 }
189
190 static void freeResultList(NetlinkList *p_list)
191 {
192    NetlinkList *l_cur;
193
194    while (p_list)
195    {
196       l_cur = p_list;
197       p_list = p_list->m_next;
198       free(l_cur->m_data);
199       free(l_cur);
200    }
201 }
202
203 static NetlinkList *getResultList(int p_socket, int p_request)
204 {
205    int l_size;
206    NetlinkList *l_list = NULL;
207    NetlinkList *l_end  = NULL;
208    int l_done          = 0;
209
210    if (netlink_send(p_socket, p_request) < 0)
211       return NULL;
212
213    while (!l_done)
214    {
215       NetlinkList *l_item    = NULL;
216       struct nlmsghdr *l_hdr = getNetlinkResponse(p_socket, &l_size, &l_done);
217       if (!l_hdr)
218          goto error;
219
220       l_item = newListItem(l_hdr, l_size);
221       if (!l_item)
222          goto error;
223
224       if (!l_list)
225          l_list        = l_item;
226       else
227          l_end->m_next = l_item;
228       l_end            = l_item;
229    }
230
231    return l_list;
232
233 error:
234    freeResultList(l_list);
235    return NULL;
236 }
237
238 static size_t maxSize(size_t a, size_t b)
239 {
240    return (a > b ? a : b);
241 }
242
243 static size_t calcAddrLen(sa_family_t p_family, int p_dataSize)
244 {
245    switch(p_family)
246    {
247       case AF_INET:
248          return sizeof(struct sockaddr_in);
249       case AF_INET6:
250          return sizeof(struct sockaddr_in6);
251       case AF_PACKET:
252          return maxSize(sizeof(struct sockaddr_ll), offsetof(struct sockaddr_ll, sll_addr) + p_dataSize);
253       default:
254          break;
255    }
256
257    return maxSize(sizeof(struct sockaddr), offsetof(struct sockaddr, sa_data) + p_dataSize);
258 }
259
260 static void makeSockaddr(sa_family_t p_family, struct sockaddr *p_dest, void *p_data, size_t p_size)
261 {
262    switch(p_family)
263    {
264       case AF_INET:
265          memcpy(&((struct sockaddr_in*)p_dest)->sin_addr, p_data, p_size);
266          break;
267       case AF_INET6:
268          memcpy(&((struct sockaddr_in6*)p_dest)->sin6_addr, p_data, p_size);
269          break;
270       case AF_PACKET:
271          memcpy(((struct sockaddr_ll*)p_dest)->sll_addr, p_data, p_size);
272          ((struct sockaddr_ll*)p_dest)->sll_halen = p_size;
273          break;
274       default:
275          memcpy(p_dest->sa_data, p_data, p_size);
276          break;
277    }
278    p_dest->sa_family = p_family;
279 }
280
281 static void addToEnd(struct ifaddrs **p_resultList, struct ifaddrs *p_entry)
282 {
283    if (!*p_resultList)
284       *p_resultList = p_entry;
285    else
286    {
287       struct ifaddrs *l_cur = *p_resultList;
288       while (l_cur->ifa_next)
289          l_cur = l_cur->ifa_next;
290       l_cur->ifa_next = p_entry;
291    }
292 }
293
294 static int interpretLink(struct nlmsghdr *p_hdr, struct ifaddrs **p_resultList)
295 {
296    struct ifaddrs *l_entry  = NULL;
297    struct rtattr *l_rta     = NULL;
298    struct ifinfomsg *l_info = (struct ifinfomsg *)NLMSG_DATA(p_hdr);
299    size_t l_nameSize        = 0;
300    size_t l_addrSize        = 0;
301    size_t l_dataSize        = 0;
302    size_t l_rtaSize         = NLMSG_PAYLOAD(p_hdr, sizeof(struct ifinfomsg));
303
304    for (l_rta = IFLA_RTA(l_info); RTA_OK(l_rta, l_rtaSize);
305          l_rta = RTA_NEXT(l_rta, l_rtaSize))
306    {
307       size_t l_rtaDataSize = RTA_PAYLOAD(l_rta);
308       switch(l_rta->rta_type)
309       {
310          case IFLA_ADDRESS:
311          case IFLA_BROADCAST:
312             l_addrSize += NLMSG_ALIGN(calcAddrLen(AF_PACKET, l_rtaDataSize));
313             break;
314          case IFLA_IFNAME:
315             l_nameSize += NLMSG_ALIGN(l_rtaSize + 1);
316             break;
317          case IFLA_STATS:
318             l_dataSize += NLMSG_ALIGN(l_rtaSize);
319             break;
320          default:
321             break;
322       }
323    }
324
325    l_entry = (struct ifaddrs*)malloc(sizeof(struct ifaddrs) + sizeof(int) + l_nameSize + l_addrSize + l_dataSize);
326    if (!l_entry)
327       return -1;
328
329    memset(l_entry, 0, sizeof(struct ifaddrs));
330    l_entry->ifa_name = "";
331
332    char *l_index = ((char *)l_entry) + sizeof(struct ifaddrs);
333    char *l_name = l_index + sizeof(int);
334    char *l_addr = l_name + l_nameSize;
335    char *l_data = l_addr + l_addrSize;
336
337    /* save the interface index so we can look 
338     * it up when handling the addresses. */
339    memcpy(l_index, &l_info->ifi_index, sizeof(int));
340
341    l_entry->ifa_flags = l_info->ifi_flags;
342
343    l_rtaSize          = NLMSG_PAYLOAD(p_hdr, sizeof(struct ifinfomsg));
344
345    for (l_rta = IFLA_RTA(l_info); RTA_OK(l_rta, l_rtaSize);
346          l_rta = RTA_NEXT(l_rta, l_rtaSize))
347    {
348       void      *l_rtaData = RTA_DATA(l_rta);
349       size_t l_rtaDataSize = RTA_PAYLOAD(l_rta);
350
351       switch(l_rta->rta_type)
352       {
353          case IFLA_ADDRESS:
354          case IFLA_BROADCAST:
355             {
356                size_t l_addrLen = calcAddrLen(AF_PACKET, l_rtaDataSize);
357                makeSockaddr(AF_PACKET, (struct sockaddr *)l_addr, l_rtaData, l_rtaDataSize);
358                ((struct sockaddr_ll *)l_addr)->sll_ifindex = l_info->ifi_index;
359                ((struct sockaddr_ll *)l_addr)->sll_hatype = l_info->ifi_type;
360                if (l_rta->rta_type == IFLA_ADDRESS)
361                   l_entry->ifa_addr      = (struct sockaddr *)l_addr;
362                else
363                   l_entry->ifa_broadaddr = (struct sockaddr *)l_addr;
364                l_addr += NLMSG_ALIGN(l_addrLen);
365                break;
366             }
367          case IFLA_IFNAME:
368             strncpy(l_name, l_rtaData, l_rtaDataSize);
369             l_name[l_rtaDataSize] = '\0';
370             l_entry->ifa_name = l_name;
371             break;
372          case IFLA_STATS:
373             memcpy(l_data, l_rtaData, l_rtaDataSize);
374             l_entry->ifa_data = l_data;
375             break;
376          default:
377             break;
378       }
379    }
380
381    addToEnd(p_resultList, l_entry);
382    return 0;
383 }
384
385 static struct ifaddrs *findInterface(int p_index,
386       struct ifaddrs **p_links, int p_numLinks)
387 {
388    int l_num             = 0;
389    struct ifaddrs *l_cur = *p_links;
390
391    while (l_cur && l_num < p_numLinks)
392    {
393       int l_index;
394       char *l_indexPtr = ((char *)l_cur) + sizeof(struct ifaddrs);
395
396       memcpy(&l_index, l_indexPtr, sizeof(int));
397       if (l_index == p_index)
398          return l_cur;
399
400       l_cur = l_cur->ifa_next;
401       ++l_num;
402    }
403    return NULL;
404 }
405
406 static int interpretAddr(struct nlmsghdr *p_hdr,
407       struct ifaddrs **p_resultList, int p_numLinks)
408 {
409    struct rtattr *l_rta;
410    size_t l_rtaSize;
411    size_t l_nameSize           = 0;
412    size_t l_addrSize           = 0;
413    int l_addedNetmask          = 0;
414    struct ifaddrmsg *l_info    = (struct ifaddrmsg *)NLMSG_DATA(p_hdr);
415    struct ifaddrs *l_interface = findInterface(l_info->ifa_index, p_resultList, p_numLinks);
416
417    if (l_info->ifa_family == AF_PACKET)
418       return 0;
419
420    l_rtaSize = NLMSG_PAYLOAD(p_hdr, sizeof(struct ifaddrmsg));
421
422    for (l_rta = IFA_RTA(l_info); RTA_OK(l_rta, l_rtaSize);
423          l_rta = RTA_NEXT(l_rta, l_rtaSize))
424    {
425       size_t l_rtaDataSize = RTA_PAYLOAD(l_rta);
426
427       switch(l_rta->rta_type)
428       {
429          case IFA_ADDRESS:
430          case IFA_LOCAL:
431             if ((l_info->ifa_family == AF_INET || l_info->ifa_family == AF_INET6) && !l_addedNetmask)
432             {
433                /* make room for netmask */
434                l_addrSize += NLMSG_ALIGN(calcAddrLen(l_info->ifa_family, l_rtaDataSize));
435                l_addedNetmask = 1;
436             }
437          case IFA_BROADCAST:
438             l_addrSize += NLMSG_ALIGN(calcAddrLen(l_info->ifa_family, l_rtaDataSize));
439             break;
440          case IFA_LABEL:
441             l_nameSize += NLMSG_ALIGN(l_rtaSize + 1);
442             break;
443          default:
444             break;
445       }
446    }
447
448    struct ifaddrs *l_entry = (struct ifaddrs*)malloc(sizeof(struct ifaddrs) + l_nameSize + l_addrSize);
449    if (!l_entry)
450       return -1;
451
452    memset(l_entry, 0, sizeof(struct ifaddrs));
453    l_entry->ifa_name = (l_interface ? l_interface->ifa_name : "");
454
455    char *l_name = ((char *)l_entry) + sizeof(struct ifaddrs);
456    char *l_addr = l_name + l_nameSize;
457
458    l_entry->ifa_flags = l_info->ifa_flags;
459    if (l_interface)
460       l_entry->ifa_flags |= l_interface->ifa_flags;
461
462    l_rtaSize = NLMSG_PAYLOAD(p_hdr, sizeof(struct ifaddrmsg));
463    for (l_rta = IFA_RTA(l_info); RTA_OK(l_rta, l_rtaSize);
464          l_rta = RTA_NEXT(l_rta, l_rtaSize))
465    {
466       void *l_rtaData = RTA_DATA(l_rta);
467       size_t l_rtaDataSize = RTA_PAYLOAD(l_rta);
468       switch(l_rta->rta_type)
469       {
470          case IFA_ADDRESS:
471          case IFA_BROADCAST:
472          case IFA_LOCAL:
473             {
474                size_t l_addrLen = calcAddrLen(l_info->ifa_family, l_rtaDataSize);
475                makeSockaddr(l_info->ifa_family, (struct sockaddr *)l_addr, l_rtaData, l_rtaDataSize);
476                if (l_info->ifa_family == AF_INET6)
477                {
478                   if (IN6_IS_ADDR_LINKLOCAL((struct in6_addr *)l_rtaData) || IN6_IS_ADDR_MC_LINKLOCAL((struct in6_addr *)l_rtaData))
479                      ((struct sockaddr_in6 *)l_addr)->sin6_scope_id = l_info->ifa_index;
480                }
481
482                if (l_rta->rta_type == IFA_ADDRESS)
483                {
484                   /* apparently in a point-to-point network IFA_ADDRESS
485                    * contains the dest address and IFA_LOCAL contains the local address */
486                   if (l_entry->ifa_addr)
487                      l_entry->ifa_dstaddr = (struct sockaddr *)l_addr;
488                   else
489                      l_entry->ifa_addr = (struct sockaddr *)l_addr;
490                }
491                else if (l_rta->rta_type == IFA_LOCAL)
492                {
493                   if (l_entry->ifa_addr)
494                      l_entry->ifa_dstaddr = l_entry->ifa_addr;
495                   l_entry->ifa_addr = (struct sockaddr *)l_addr;
496                }
497                else
498                   l_entry->ifa_broadaddr = (struct sockaddr *)l_addr;
499                l_addr += NLMSG_ALIGN(l_addrLen);
500                break;
501             }
502          case IFA_LABEL:
503             strncpy(l_name, l_rtaData, l_rtaDataSize);
504             l_name[l_rtaDataSize] = '\0';
505             l_entry->ifa_name = l_name;
506             break;
507          default:
508             break;
509       }
510    }
511
512    if (l_entry->ifa_addr &&
513          (   l_entry->ifa_addr->sa_family == AF_INET
514           || l_entry->ifa_addr->sa_family == AF_INET6))
515    {
516       unsigned i;
517       char l_mask[16];
518       unsigned l_maxPrefix = (l_entry->ifa_addr->sa_family == AF_INET
519             ? 32 : 128);
520       unsigned l_prefix    = (l_info->ifa_prefixlen > l_maxPrefix
521             ? l_maxPrefix : l_info->ifa_prefixlen);
522
523       l_mask[0] = '\0';
524
525       for (i=0; i<(l_prefix/8); ++i)
526          l_mask[i] = 0xff;
527       if (l_prefix % 8)
528          l_mask[i] = 0xff << (8 - (l_prefix % 8));
529
530       makeSockaddr(l_entry->ifa_addr->sa_family,
531             (struct sockaddr *)l_addr, l_mask, l_maxPrefix / 8);
532       l_entry->ifa_netmask = (struct sockaddr *)l_addr;
533    }
534
535    addToEnd(p_resultList, l_entry);
536    return 0;
537 }
538
539 static int interpretLinks(int p_socket, NetlinkList *p_netlinkList,
540       struct ifaddrs **p_resultList)
541 {
542    int l_numLinks = 0;
543    pid_t l_pid    = getpid();
544
545    for (; p_netlinkList; p_netlinkList = p_netlinkList->m_next)
546    {
547       struct nlmsghdr *l_hdr = NULL;
548       unsigned int l_nlsize  = p_netlinkList->m_size;
549
550       for (l_hdr = p_netlinkList->m_data; NLMSG_OK(l_hdr, l_nlsize);
551             l_hdr = NLMSG_NEXT(l_hdr, l_nlsize))
552       {
553          if (  (pid_t)l_hdr->nlmsg_pid != l_pid || 
554                (int)l_hdr->nlmsg_seq   != p_socket)
555             continue;
556
557          if (l_hdr->nlmsg_type == NLMSG_DONE)
558             break;
559
560          if (l_hdr->nlmsg_type == RTM_NEWLINK)
561          {
562             if (interpretLink(l_hdr, p_resultList) == -1)
563                return -1;
564             ++l_numLinks;
565          }
566       }
567    }
568    return l_numLinks;
569 }
570
571 static int interpretAddrs(int p_socket, NetlinkList *p_netlinkList,
572       struct ifaddrs **p_resultList, int p_numLinks)
573 {
574    pid_t l_pid = getpid();
575    for (; p_netlinkList; p_netlinkList = p_netlinkList->m_next)
576    {
577       struct nlmsghdr *l_hdr = NULL;
578       unsigned int l_nlsize  = p_netlinkList->m_size;
579
580       for (l_hdr = p_netlinkList->m_data; NLMSG_OK(l_hdr, l_nlsize);
581             l_hdr = NLMSG_NEXT(l_hdr, l_nlsize))
582       {
583          if (     (pid_t)l_hdr->nlmsg_pid != l_pid 
584                || (int)l_hdr->nlmsg_seq   != p_socket)
585             continue;
586
587          if (l_hdr->nlmsg_type == NLMSG_DONE)
588             break;
589
590          if (l_hdr->nlmsg_type == RTM_NEWADDR)
591          {
592             if (interpretAddr(l_hdr, p_resultList, p_numLinks) == -1)
593                return -1;
594          }
595       }
596    }
597    return 0;
598 }
599
600 int getifaddrs(struct ifaddrs **ifap)
601 {
602    NetlinkList *l_linkResults;
603    NetlinkList *l_addrResults;
604    int l_numLinks;
605    int l_socket   = 0;
606    int l_result   = 0;
607    if (!ifap)
608       return -1;
609
610    *ifap    = NULL;
611
612    l_socket = netlink_socket();
613
614    if (l_socket < 0)
615       return -1;
616
617    l_linkResults = getResultList(l_socket, RTM_GETLINK);
618    if (!l_linkResults)
619    {
620       close(l_socket);
621       return -1;
622    }
623
624    l_addrResults = getResultList(l_socket, RTM_GETADDR);
625    if (!l_addrResults)
626    {
627       close(l_socket);
628       freeResultList(l_linkResults);
629       return -1;
630    }
631
632    l_numLinks = interpretLinks(l_socket, l_linkResults, ifap);
633
634    if (  l_numLinks == -1 || 
635          interpretAddrs(l_socket, l_addrResults, ifap, l_numLinks) == -1)
636       l_result = -1;
637
638    freeResultList(l_linkResults);
639    freeResultList(l_addrResults);
640    close(l_socket);
641    return l_result;
642 }
643
644 void freeifaddrs(struct ifaddrs *ifa)
645 {
646    struct ifaddrs *l_cur = NULL;
647
648    while (ifa)
649    {
650       l_cur = ifa;
651       ifa   = ifa->ifa_next;
652       free(l_cur);
653    }
654 }