utils.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2022 Felix Fietkau <nbd@nbd.name>
   4  */
   5 #include <sys/types.h>
   6 #include <sys/socket.h>
   7 #include <sys/stat.h>
   8 #include <arpa/inet.h>
   9 #include <netdb.h>
  10 #include <stdio.h>
  11 #include <netinet/in.h>
  12 #include <netinet/ip.h>
  13 #include <netinet/ip6.h>
  14 #include <netinet/udp.h>
  15
  16 #include <libubox/utils.h>
  17
  18 #include "unetd.h"
  19
  20 int network_get_endpoint(union network_endpoint *dest, int af, const char *str,
  21                          int default_port, int idx)
  22 {
  23         struct addrinfo hints = {
  24                 .ai_flags = AI_ADDRCONFIG,
  25                 .ai_family = af,
  26         };
  27         char *buf = strdup(str);
  28         char *host = buf, *port;
  29         struct addrinfo *ai, *ai_cur;
  30         int n_res;
  31         int ret = -1;
  32
  33         memset(dest, 0, sizeof(*dest));
  34
  35         if (*host == '[') {
  36                 if (af == AF_INET)
  37                         goto out;
  38
  39                 host++;
  40                 port = strchr(host, ']');
  41                 if (!port)
  42                         goto out;
  43
  44                 *(port++) = 0;
  45                 if (!*port)
  46                         port = NULL;
  47                 else if (*port == ':')
  48                         port++;
  49                 else
  50                         goto out;
  51                 hints.ai_family = AF_INET6;
  52                 hints.ai_flags |= AI_NUMERICHOST;
  53         } else {
  54                 host = buf;
  55
  56                 port = strchr(host, ':');
  57                 if (port)
  58                         *(port++) = 0;
  59         }
  60
  61         if (getaddrinfo(host, port, &hints, &ai) || !ai)
  62                 goto out;
  63
  64         while (1) {
  65                 ai_cur = ai;
  66                 for (n_res = 0; ai_cur; ai_cur = ai_cur->ai_next, n_res++)
  67                         if (!idx--)
  68                                 goto found;
  69
  70                 idx %= n_res;
  71         }
  72
  73 found:
  74         if (ai_cur->ai_addrlen > sizeof(*dest))
  75                 goto free_ai;
  76
  77         memcpy(dest, ai_cur->ai_addr, ai_cur->ai_addrlen);
  78         if (!port)
  79                 dest->in.sin_port = htons(default_port);
  80         ret = 0;
  81
  82 free_ai:
  83         freeaddrinfo(ai);
  84
  85 out:
  86         free(buf);
  87         return ret;
  88 }
  89
  90 int network_get_subnet(int af, union network_addr *addr, int *mask, const char *str)
  91 {
  92         char *buf = strdup(str);
  93         char *sep, *end;
  94         int ret = -1;
  95
  96         if (af == AF_INET6)
  97                 *mask = 128;
  98         else
  99                 *mask = 32;
 100
 101         sep = strchr(buf, '/');
 102         if (sep) {
 103                 unsigned long val;
 104
 105                 *(sep++) = 0;
 106
 107                 val = strtoul(sep, &end, 0);
 108                 if ((end && *end) || val > *mask)
 109                         goto out;
 110
 111                 *mask = val;
 112         }
 113
 114         if (inet_pton(af, buf, addr) == 1)
 115                 ret = 0;
 116
 117 out:
 118         free(buf);
 119         return ret;
 120 }
 121
 122 int network_get_local_addr(void *local, const union network_endpoint *target)
 123 {
 124         union network_endpoint ep = {};
 125         socklen_t len;
 126         int ret = -1;
 127         int fd;
 128
 129         memset(local, 0, sizeof(union network_addr));
 130         if (target->sa.sa_family == AF_INET6)
 131                 len = sizeof(ep.in6);
 132         else
 133                 len = sizeof(ep.in);
 134
 135         fd = socket(target->sa.sa_family, SOCK_DGRAM, IPPROTO_UDP);
 136         if (fd < 0)
 137                 return -1;
 138
 139         if (connect(fd, (const struct sockaddr *)target, len))
 140                 goto out;
 141
 142         len = sizeof(ep);
 143         if (getsockname(fd, &ep.sa, &len))
 144                 goto out;
 145
 146         if (ep.sa.sa_family == AF_INET6)
 147                 memcpy(local, &ep.in6.sin6_addr, sizeof(ep.in6.sin6_addr));
 148         else
 149                 memcpy(local, &ep.in.sin_addr, sizeof(ep.in.sin_addr));
 150         ret = 0;
 151
 152 out:
 153         close(fd);
 154         return ret;
 155 }
 156
 157 void *unet_read_file(const char *name, size_t *len)
 158 {
 159         struct stat st;
 160         void *data;
 161         FILE *f;
 162
 163         f = fopen(name, "r");
 164         if (!f)
 165                 goto error;
 166
 167         if (fstat(fileno(f), &st) < 0)
 168                 goto close;
 169
 170         if (*len && st.st_size > *len)
 171                 goto close;
 172
 173         data = malloc(st.st_size);
 174         if (!data)
 175                 goto close;
 176
 177         if (fread(data, 1, st.st_size, f) != st.st_size) {
 178                 free(data);
 179                 goto close;
 180         }
 181         fclose(f);
 182
 183         *len = st.st_size;
 184         return data;
 185
 186 close:
 187         fclose(f);
 188 error:
 189         *len = 0;
 190         return NULL;
 191 }
 192
 193 uint64_t unet_gettime(void)
 194 {
 195         struct timespec ts;
 196
 197         clock_gettime(CLOCK_MONOTONIC, &ts);
 198
 199         return ts.tv_sec;
 200 }
 201
 202 static inline uint32_t
 203 csum_tcpudp_nofold(uint32_t saddr, uint32_t daddr, uint8_t proto, uint32_t len)
 204 {
 205         uint64_t sum = 0;
 206
 207         sum += saddr;
 208         sum += daddr;
 209 #if __BYTE_ORDER == __LITTLE_ENDIAN
 210         sum += (proto + len) << 8;
 211 #else
 212         sum += proto + len;
 213 #endif
 214
 215         sum = (sum & 0xffffffff) + (sum >> 32);
 216         sum = (sum & 0xffffffff) + (sum >> 32);
 217
 218         return (uint32_t)sum;
 219 }
 220
 221 static inline uint32_t csum_add(uint32_t sum, uint32_t addend)
 222 {
 223         sum += addend;
 224         return sum + (sum < addend);
 225 }
 226
 227 static inline uint16_t csum_fold(uint32_t sum)
 228 {
 229         sum = (sum & 0xffff) + (sum >> 16);
 230         sum = (sum & 0xffff) + (sum >> 16);
 231
 232         return (uint16_t)~sum;
 233 }
 234
 235 static uint32_t csum_partial(const void *buf, int len)
 236 {
 237         const uint16_t *data = buf;
 238         uint32_t sum = 0;
 239
 240         while (len > 1) {
 241                 sum += *data++;
 242                 len -= 2;
 243         }
 244
 245         if (len == 1)
 246 #if __BYTE_ORDER == __LITTLE_ENDIAN
 247                 sum += *(uint8_t *)data;
 248 #else
 249                 sum += *(uint8_t *)data << 8;
 250 #endif
 251
 252         sum = (sum & 0xffff) + (sum >> 16);
 253         sum = (sum & 0xffff) + (sum >> 16);
 254
 255         return sum;
 256 }
 257
 258 static void fixup_udpv4(void *hdr, size_t hdrlen, const void *data, size_t len)
 259 {
 260         struct ip *ip = hdr;
 261         struct udphdr *udp = hdr + ip->ip_hl * 4;
 262         uint16_t udp_len = sizeof(*udp) + len;
 263         uint32_t sum;
 264
 265         if ((void *)&udp[1] > hdr + hdrlen)
 266                 return;
 267
 268         udp->uh_sum = 0;
 269         udp->uh_ulen = htons(udp_len);
 270         sum = csum_tcpudp_nofold(*(uint32_t *)&ip->ip_src, *(uint32_t *)&ip->ip_dst,
 271                                  ip->ip_p, udp_len);
 272         sum = csum_add(sum, csum_partial(udp, sizeof(*udp)));
 273         sum = csum_add(sum, csum_partial(data, len));
 274         udp->uh_sum = csum_fold(sum);
 275
 276         ip->ip_len = htons(hdrlen + len);
 277         ip->ip_sum = 0;
 278         ip->ip_sum = csum_fold(csum_partial(ip, sizeof(*ip)));
 279
 280 #ifdef __APPLE__
 281         ip->ip_len = hdrlen + len;
 282 #endif
 283 }
 284
 285 static void fixup_udpv6(void *hdr, size_t hdrlen, const void *data, size_t len)
 286 {
 287         struct ip6_hdr *ip = hdr;
 288         struct udphdr *udp = hdr + sizeof(*ip);
 289         uint16_t udp_len = htons(sizeof(*udp) + len);
 290
 291         if ((void *)&udp[1] > hdr + hdrlen)
 292                 return;
 293
 294         ip->ip6_plen = htons(sizeof(*udp) + len);
 295         udp->uh_sum = 0;
 296         udp->uh_ulen = udp_len;
 297         udp->uh_sum = csum_fold(csum_partial(hdr, sizeof(*ip) + sizeof(*udp)));
 298
 299 #ifdef __APPLE__
 300         ip->ip6_plen = sizeof(*udp) + len;
 301 #endif
 302 }
 303
 304 static void fixup_ip_udp_header(void *hdr, size_t hdrlen, const void *data, size_t len)
 305 {
 306         if (hdrlen >= sizeof(struct ip6_hdr) + sizeof(struct udphdr))
 307                 fixup_udpv6(hdr, hdrlen, data, len);
 308         else if (hdrlen >= sizeof(struct ip) + sizeof(struct udphdr))
 309                 fixup_udpv4(hdr, hdrlen, data, len);
 310 }
 311
 312 int sendto_rawudp(int fd, const void *addr, void *ip_hdr, size_t ip_hdrlen,
 313                   const void *data, size_t len)
 314 {
 315         const struct sockaddr *sa = addr;
 316         struct iovec iov[2] = {
 317                 { .iov_base = ip_hdr, .iov_len = ip_hdrlen },
 318                 { .iov_base = (void *)data, .iov_len = len }
 319         };
 320         struct msghdr msg = {
 321                 .msg_name = (void *)addr,
 322                 .msg_iov = iov,
 323                 .msg_iovlen = ARRAY_SIZE(iov),
 324         };
 325
 326         if (sa->sa_family == AF_INET6)
 327                 msg.msg_namelen = sizeof(struct sockaddr_in6);
 328         else
 329                 msg.msg_namelen = sizeof(struct sockaddr_in);
 330
 331         fixup_ip_udp_header(ip_hdr, ip_hdrlen, data, len);
 332
 333         return sendmsg(fd, &msg, 0);
 334 }