123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334 |
- // SPDX-License-Identifier: GPL-2.0-or-later
- /*
- * Copyright (C) 2022 Felix Fietkau <nbd@nbd.name>
- */
- #include <sys/types.h>
- #include <sys/socket.h>
- #include <sys/stat.h>
- #include <arpa/inet.h>
- #include <netdb.h>
- #include <stdio.h>
- #include <netinet/in.h>
- #include <netinet/ip.h>
- #include <netinet/ip6.h>
- #include <netinet/udp.h>
- #include <libubox/utils.h>
- #include "unetd.h"
- int network_get_endpoint(union network_endpoint *dest, int af, const char *str,
- int default_port, int idx)
- {
- struct addrinfo hints = {
- .ai_flags = AI_ADDRCONFIG,
- .ai_family = af,
- };
- char *buf = strdup(str);
- char *host = buf, *port;
- struct addrinfo *ai, *ai_cur;
- int n_res;
- int ret = -1;
- memset(dest, 0, sizeof(*dest));
- if (*host == '[') {
- if (af == AF_INET)
- goto out;
- host++;
- port = strchr(host, ']');
- if (!port)
- goto out;
- *(port++) = 0;
- if (!*port)
- port = NULL;
- else if (*port == ':')
- port++;
- else
- goto out;
- hints.ai_family = AF_INET6;
- hints.ai_flags |= AI_NUMERICHOST;
- } else {
- host = buf;
- port = strchr(host, ':');
- if (port)
- *(port++) = 0;
- }
- if (getaddrinfo(host, port, &hints, &ai) || !ai)
- goto out;
- while (1) {
- ai_cur = ai;
- for (n_res = 0; ai_cur; ai_cur = ai_cur->ai_next, n_res++)
- if (!idx--)
- goto found;
- idx %= n_res;
- }
- found:
- if (ai_cur->ai_addrlen > sizeof(*dest))
- goto free_ai;
- memcpy(dest, ai_cur->ai_addr, ai_cur->ai_addrlen);
- if (!port)
- dest->in.sin_port = htons(default_port);
- ret = 0;
- free_ai:
- freeaddrinfo(ai);
- out:
- free(buf);
- return ret;
- }
- int network_get_subnet(int af, union network_addr *addr, int *mask, const char *str)
- {
- char *buf = strdup(str);
- char *sep, *end;
- int ret = -1;
- if (af == AF_INET6)
- *mask = 128;
- else
- *mask = 32;
- sep = strchr(buf, '/');
- if (sep) {
- unsigned long val;
- *(sep++) = 0;
- val = strtoul(sep, &end, 0);
- if ((end && *end) || val > *mask)
- goto out;
- *mask = val;
- }
- if (inet_pton(af, buf, addr) == 1)
- ret = 0;
- out:
- free(buf);
- return ret;
- }
- int network_get_local_addr(void *local, const union network_endpoint *target)
- {
- union network_endpoint ep = {};
- socklen_t len;
- int ret = -1;
- int fd;
- memset(local, 0, sizeof(union network_addr));
- if (target->sa.sa_family == AF_INET6)
- len = sizeof(ep.in6);
- else
- len = sizeof(ep.in);
- fd = socket(target->sa.sa_family, SOCK_DGRAM, IPPROTO_UDP);
- if (fd < 0)
- return -1;
- if (connect(fd, (const struct sockaddr *)target, len))
- goto out;
- len = sizeof(ep);
- if (getsockname(fd, &ep.sa, &len))
- goto out;
- if (ep.sa.sa_family == AF_INET6)
- memcpy(local, &ep.in6.sin6_addr, sizeof(ep.in6.sin6_addr));
- else
- memcpy(local, &ep.in.sin_addr, sizeof(ep.in.sin_addr));
- ret = 0;
- out:
- close(fd);
- return ret;
- }
- void *unet_read_file(const char *name, size_t *len)
- {
- struct stat st;
- void *data;
- FILE *f;
- f = fopen(name, "r");
- if (!f)
- goto error;
- if (fstat(fileno(f), &st) < 0)
- goto close;
- if (*len && st.st_size > *len)
- goto close;
- data = malloc(st.st_size);
- if (!data)
- goto close;
- if (fread(data, 1, st.st_size, f) != st.st_size) {
- free(data);
- goto close;
- }
- fclose(f);
- *len = st.st_size;
- return data;
- close:
- fclose(f);
- error:
- *len = 0;
- return NULL;
- }
- uint64_t unet_gettime(void)
- {
- struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC, &ts);
- return ts.tv_sec;
- }
- static inline uint32_t
- csum_tcpudp_nofold(uint32_t saddr, uint32_t daddr, uint8_t proto, uint32_t len)
- {
- uint64_t sum = 0;
- sum += saddr;
- sum += daddr;
- #if __BYTE_ORDER == __LITTLE_ENDIAN
- sum += (proto + len) << 8;
- #else
- sum += proto + len;
- #endif
- sum = (sum & 0xffffffff) + (sum >> 32);
- sum = (sum & 0xffffffff) + (sum >> 32);
- return (uint32_t)sum;
- }
- static inline uint32_t csum_add(uint32_t sum, uint32_t addend)
- {
- sum += addend;
- return sum + (sum < addend);
- }
- static inline uint16_t csum_fold(uint32_t sum)
- {
- sum = (sum & 0xffff) + (sum >> 16);
- sum = (sum & 0xffff) + (sum >> 16);
- return (uint16_t)~sum;
- }
- static uint32_t csum_partial(const void *buf, int len)
- {
- const uint16_t *data = buf;
- uint32_t sum = 0;
- while (len > 1) {
- sum += *data++;
- len -= 2;
- }
- if (len == 1)
- #if __BYTE_ORDER == __LITTLE_ENDIAN
- sum += *(uint8_t *)data;
- #else
- sum += *(uint8_t *)data << 8;
- #endif
- sum = (sum & 0xffff) + (sum >> 16);
- sum = (sum & 0xffff) + (sum >> 16);
- return sum;
- }
- static void fixup_udpv4(void *hdr, size_t hdrlen, const void *data, size_t len)
- {
- struct ip *ip = hdr;
- struct udphdr *udp = hdr + ip->ip_hl * 4;
- uint16_t udp_len = sizeof(*udp) + len;
- uint32_t sum;
- if ((void *)&udp[1] > hdr + hdrlen)
- return;
- udp->uh_sum = 0;
- udp->uh_ulen = htons(udp_len);
- sum = csum_tcpudp_nofold(*(uint32_t *)&ip->ip_src, *(uint32_t *)&ip->ip_dst,
- ip->ip_p, udp_len);
- sum = csum_add(sum, csum_partial(udp, sizeof(*udp)));
- sum = csum_add(sum, csum_partial(data, len));
- udp->uh_sum = csum_fold(sum);
- ip->ip_len = htons(hdrlen + len);
- ip->ip_sum = 0;
- ip->ip_sum = csum_fold(csum_partial(ip, sizeof(*ip)));
- #ifdef __APPLE__
- ip->ip_len = hdrlen + len;
- #endif
- }
- static void fixup_udpv6(void *hdr, size_t hdrlen, const void *data, size_t len)
- {
- struct ip6_hdr *ip = hdr;
- struct udphdr *udp = hdr + sizeof(*ip);
- uint16_t udp_len = htons(sizeof(*udp) + len);
- if ((void *)&udp[1] > hdr + hdrlen)
- return;
- ip->ip6_plen = htons(sizeof(*udp) + len);
- udp->uh_sum = 0;
- udp->uh_ulen = udp_len;
- udp->uh_sum = csum_fold(csum_partial(hdr, sizeof(*ip) + sizeof(*udp)));
- #ifdef __APPLE__
- ip->ip6_plen = sizeof(*udp) + len;
- #endif
- }
- static void fixup_ip_udp_header(void *hdr, size_t hdrlen, const void *data, size_t len)
- {
- if (hdrlen >= sizeof(struct ip6_hdr) + sizeof(struct udphdr))
- fixup_udpv6(hdr, hdrlen, data, len);
- else if (hdrlen >= sizeof(struct ip) + sizeof(struct udphdr))
- fixup_udpv4(hdr, hdrlen, data, len);
- }
- int sendto_rawudp(int fd, const void *addr, void *ip_hdr, size_t ip_hdrlen,
- const void *data, size_t len)
- {
- const struct sockaddr *sa = addr;
- struct iovec iov[2] = {
- { .iov_base = ip_hdr, .iov_len = ip_hdrlen },
- { .iov_base = (void *)data, .iov_len = len }
- };
- struct msghdr msg = {
- .msg_name = (void *)addr,
- .msg_iov = iov,
- .msg_iovlen = ARRAY_SIZE(iov),
- };
- if (sa->sa_family == AF_INET6)
- msg.msg_namelen = sizeof(struct sockaddr_in6);
- else
- msg.msg_namelen = sizeof(struct sockaddr_in);
- fixup_ip_udp_header(ip_hdr, ip_hdrlen, data, len);
- return sendmsg(fd, &msg, 0);
- }
|