diff options
Diffstat (limited to 'shared/n-dhcp4/src/util/packet.c')
| -rw-r--r-- | shared/n-dhcp4/src/util/packet.c | 448 |
1 files changed, 0 insertions, 448 deletions
diff --git a/shared/n-dhcp4/src/util/packet.c b/shared/n-dhcp4/src/util/packet.c deleted file mode 100644 index 48f2c85e..00000000 --- a/shared/n-dhcp4/src/util/packet.c +++ /dev/null @@ -1,448 +0,0 @@ -/* - * Packet Sockets - */ - -#include <assert.h> -#include <c-stdaux.h> -#include <endian.h> -#include <errno.h> -#include <linux/filter.h> -#include <linux/if_ether.h> -#include <linux/if_packet.h> -#include <linux/udp.h> -#include <netinet/in.h> -#include <netinet/ip.h> -#include <stdbool.h> -#include <stdlib.h> -#include <string.h> -#include <sys/types.h> -#include <sys/socket.h> -#include "packet.h" - -/** - * packet_internet_checksum() - compute the internet checksum - * @data: the data to checksum - * @size: the length of @data in bytes - * - * Computes the internet checksum for a given blob according to RFC1071. - * - * The internet checksum is the one's complement of the one's complement sum of - * the 16-bit words of the data, padded with zero-bytes if the data does not - * end on a 16-bit boundary. - * - * Return: Checksum is returned. - */ -uint16_t packet_internet_checksum(const uint8_t *data, size_t size) { - uint64_t acc = 0; - uint32_t local; - - while (size >= sizeof(local)) { - memcpy(&local, data, sizeof(local)); - acc += local; - - data += sizeof(local); - size -= sizeof(local); - } - - if (size) { - local = 0; - memcpy(&local, data, size); - acc += local; - } - - while (acc >> 16) - acc = (acc & 0xffff) + (acc >> 16); - - return ~acc; -} - -/** - * packet_internet_checksum_udp() - compute the internet checkum for UDP packets - * @src_addr: source IP address - * @dst_addr: destination IP address - * @src_port: source port - * @dst_port: destination port - * @data: payload - * @size: length of payload in bytes - * @checksum: current checksum, or 0 - * - * Computes the internet checksum for a UDP packet, given the relevant IP and - * UDP header fields. - * - * Note that since a UDP packet contains the checksum itself, the resulting - * checksum will always be 0 (this fact is used to verify that a UDP packet is - * valid). - * Inversely, when calculating the checksum for outgoing packets, you have to - * specify 0 as @checksum, and this function will return the checksum for the - * caller to use for the packet. In this case, though, the caller must check - * whether the returned checksum might coincidentally be 0, in which case it - * must be flipped to -1 (0xffff), since 0 is not allowed as checksum in UDP - * packets, and -1 is arithmetically equivalent in the checksum calculation. - * - * Return: Checksum is returned. - */ -uint16_t packet_internet_checksum_udp(const struct in_addr *src_addr, - const struct in_addr *dst_addr, - uint16_t src_port, - uint16_t dst_port, - const uint8_t *data, - size_t size, - uint16_t checksum) { - struct { - uint32_t src; - uint32_t dst; - uint8_t _zeros; - uint8_t protocol; - uint16_t length; - struct udphdr udp; - } _c_packed_ udp_phdr = { - .src = src_addr->s_addr, - .dst = dst_addr->s_addr, - .protocol = IPPROTO_UDP, - .length = htons(sizeof(struct udphdr) + size), - .udp = { - .source = htons(src_port), - .dest = htons(dst_port), - .len = htons(sizeof(struct udphdr) + size), - .check = checksum, - }, - }; - const uint8_t *iter; - uint64_t acc = 0; - uint32_t local; - - _Static_assert(!(sizeof(udp_phdr) % sizeof(local)), - "UDP header structure size is not a multiple of 4"); - - for (iter = (const uint8_t *)&udp_phdr; - iter < (const uint8_t *)(&udp_phdr + 1); - iter += sizeof(local)) { - memcpy(&local, iter, sizeof(local)); - acc += local; - } - - while (size >= sizeof(local)) { - memcpy(&local, data, sizeof(local)); - acc += local; - - data += sizeof(local); - size -= sizeof(local); - } - - if (size) { - local = 0; - memcpy(&local, data, size); - acc += local; - } - - while (acc >> 16) - acc = (acc & 0xffff) + (acc >> 16); - - return ~acc; -} - -/** - * packet_sendto_udp() - send UDP packet on AF_PACKET socket - * @sockfd: AF_PACKET/SOCK_DGRAM socket - * @buf: payload - * @n_buf: length of payload in bytes - * @n_transmittedp: output argument for number of transmitted bytes - * @src_paddr: source protocol address, see ip(7) - * @dest_haddr: destination hardware address, see packet(7) - * @dest_paddr: destination protocol address, see ip(7) - * - * Sends an UDP packet on a AF_PACKET socket directly to a hardware - * address. The difference between this and sendto() on an AF_INET - * socket is that no routing is performed, so the packet is delivered - * even if the destination IP is not yet configured on the destination - * host. - * - * Return: 0 on success, negative error code on failure. - */ -int packet_sendto_udp(int sockfd, - const void *buf, - size_t n_buf, - size_t *n_transmittedp, - const struct sockaddr_in *src_paddr, - const struct packet_sockaddr_ll *dest_haddr, - const struct sockaddr_in *dest_paddr) { - struct iphdr ip_hdr = { - .version = IPVERSION, - .ihl = sizeof(ip_hdr) / 4, /* Length of header in multiples of four bytes */ - .tos = IPTOS_CLASS_CS6, /* Class Selector for network control */ - .tot_len = htons(sizeof(struct iphdr) + sizeof(struct udphdr) + n_buf), - .frag_off = htons(IP_DF), /* Do not fragment */ - .ttl = IPDEFTTL, - .protocol = IPPROTO_UDP, - .saddr = src_paddr->sin_addr.s_addr, - .daddr = dest_paddr->sin_addr.s_addr, - }; - struct udphdr udp_hdr = { - .source = src_paddr->sin_port, - .dest = dest_paddr->sin_port, - .len = htons(sizeof(udp_hdr) + n_buf), - }; - struct iovec iov[3] = { - { - .iov_base = &ip_hdr, - .iov_len = sizeof(ip_hdr), - }, - { - .iov_base = &udp_hdr, - .iov_len = sizeof(udp_hdr), - }, - { - .iov_base = (void *)buf, - .iov_len = n_buf, - }, - }; - struct msghdr msg = { - .msg_name = (void*)dest_haddr, - .msg_namelen = sizeof(*dest_haddr), - .msg_iov = iov, - .msg_iovlen = sizeof(iov) / sizeof(iov[0]), - }; - ssize_t pktlen; - - ip_hdr.check = packet_internet_checksum((void*)&ip_hdr, sizeof(ip_hdr)); - udp_hdr.check = packet_internet_checksum_udp(&src_paddr->sin_addr, - &dest_paddr->sin_addr, - ntohs(src_paddr->sin_port), - ntohs(dest_paddr->sin_port), - buf, - n_buf, - 0); - - /* - * 0x0000 and 0xffff are equivalent for computing the UDP checksum, - * but 0x0000 is reserved in UDP headers, to mean that the checksum is - * not set and should be ignored by the receiver. Hence, flip it to - * 0xffff in that case. - */ - udp_hdr.check = udp_hdr.check ?: 0xffff; - - pktlen = sendmsg(sockfd, &msg, 0); - if (pktlen < 0) - return -c_errno(); - - /* - * Kernel never truncates. Worst case, we get -EMSGSIZE. Kernel *might* - * prepend VNET headers, in which case a bigger length than sent is - * returned. - * Lets assert on this, and then return to the caller the proportion of - * its own buffer that we sent (which is always exactly the requested - * size). - */ - c_assert((size_t)pktlen >= sizeof(ip_hdr) + sizeof(udp_hdr) + n_buf); - *n_transmittedp = n_buf; - return 0; -} - -/** - * packet_recvfrom_upd() - receive UDP packet from AF_PACKET socket - * @sockfd: AF_PACKET/SOCK_DGRAM socket - * @buf: buffor for payload - * @n_buf: max length of payload in bytes - * @n_transmittedp: output argument for number transmitted bytes - * @src: return argument for source address, or NULL, see ip(7) - * - * Receives an UDP packet on a AF_PACKET socket. The difference between - * this and recvfrom() on an AF_INET socket is that the packet will be - * received even if the destination IP address has not been configured - * on the interface. - * - * Return: 0 on success, negative error code on failure. - */ -int packet_recvfrom_udp(int sockfd, - void *buf, - size_t n_buf, - size_t *n_transmittedp, - struct sockaddr_in *src) { - union { - struct iphdr hdr; - /* - * Maximum IP-header length is 15 * 4, since it is specified in - * the `ihl` field, which is four bits and interpreted as - * factor of 4. So maximum `ihl` value is `(2^4 - 1) * 4`. - */ - uint8_t data[15 * 4]; - } ip_hdr; - struct udphdr udp_hdr; - struct iovec iov[3] = { - { - .iov_base = &ip_hdr, - }, - { - .iov_base = &udp_hdr, - .iov_len = sizeof(udp_hdr), - }, - { - .iov_base = buf, - .iov_len = n_buf, - }, - }; - uint8_t cmsgbuf[CMSG_LEN(sizeof(struct tpacket_auxdata))]; - struct msghdr msg = { - .msg_iov = iov, - .msg_iovlen = sizeof(iov) / sizeof(iov[0]), - .msg_control = cmsgbuf, - .msg_controllen = sizeof(cmsgbuf), - }; - struct cmsghdr *cmsg; - bool checksum = true; - ssize_t pktlen; - size_t hdrlen; - - *n_transmittedp = 0; - - /* Peek packet to obtain the real IP header length */ - pktlen = recv(sockfd, &ip_hdr.hdr, sizeof(ip_hdr.hdr), MSG_PEEK); - if (pktlen < 0) - return -errno; - - if ((size_t)pktlen < sizeof(ip_hdr.hdr)) { - /* - * Received packet is smaller than the minimal IP header length, - * discard it. - */ - recv(sockfd, NULL, 0, 0); - return 0; - } - - if (ip_hdr.hdr.version != IPVERSION) { - /* - * This is not an IPv4 packet, discard it. - */ - recv(sockfd, NULL, 0, 0); - return 0; - } - - hdrlen = ip_hdr.hdr.ihl * 4; - if (hdrlen < sizeof(ip_hdr.hdr)) { - /* - * The length given in the header is smaller than the minimum - * header length, discard the packet. - */ - recv(sockfd, NULL, 0, 0); - return 0; - } - - /* - * Now that we know the ip-header length, we can prepare the iovec to - * read the entire packet into the correct buffers. - */ - iov[0].iov_len = hdrlen; - pktlen = recvmsg(sockfd, &msg, 0); - if (pktlen < 0) - return -errno; - - cmsg = CMSG_FIRSTHDR(&msg); - if (cmsg) { - if (cmsg->cmsg_level == SOL_PACKET && - cmsg->cmsg_type == PACKET_AUXDATA && - cmsg->cmsg_len == CMSG_LEN(sizeof(struct tpacket_auxdata))) { - struct tpacket_auxdata *aux = (void *)CMSG_DATA(cmsg); - checksum = !(aux->tp_status & TP_STATUS_CSUMNOTREADY); - } - } - - if (ntohs(ip_hdr.hdr.tot_len) > pktlen) { - /* - * The IP-packet is bigger than the chunk returned by the - * kernel. So either the packet is corrupt, or our caller - * provided too small a buffer. In both cases, we simply drop - * the packet. - */ - return 0; - } - - /* Truncate trailing garbage. */ - pktlen = ntohs(ip_hdr.hdr.tot_len); - - if ((size_t)pktlen < hdrlen + sizeof(udp_hdr)) { - /* - * The packet is too small to even contain an entire UDP - * header, so discard it entirely. - */ - return 0; - } else if ((size_t)pktlen < hdrlen + ntohs(udp_hdr.len)) { - /* - * The UDP header specified a longer length than the returned - * packet, so discard it entirely. - */ - return 0; - } - - /* - * Make @pktlen the length of the packet payload, without IP/UDP - * headers, since that is what the caller is interested in. - */ - pktlen = ntohs(udp_hdr.len) - sizeof(struct udphdr); - - /* IP */ - - if (ip_hdr.hdr.protocol != IPPROTO_UDP) { - return 0; /* not a UDP packet, discard it */ - } else if (ip_hdr.hdr.frag_off & htons(IP_MF | IP_OFFMASK)) { - return 0; /* fragmented packet, discard it */ - } else if (checksum && packet_internet_checksum(ip_hdr.data, hdrlen)) { - return 0; /* invalid checksum, discard it */ - } - - /* UDP */ - - if (checksum && udp_hdr.check) { - /* - * Computing the checksum of a packet that has the checksum set - * must yield 0. If it does not yield 0, the packet is invalid, - * in which case we discard it. - */ - if (packet_internet_checksum_udp(&(struct in_addr){ ip_hdr.hdr.saddr }, - &(struct in_addr){ ip_hdr.hdr.daddr }, - ntohs(udp_hdr.source), - ntohs(udp_hdr.dest), - buf, - pktlen, - udp_hdr.check)) { - return 0; - } - } - - if (src) { - src->sin_family = AF_INET; - src->sin_addr.s_addr = ip_hdr.hdr.saddr; - src->sin_port = udp_hdr.source; - } - - /* Return length of UDP payload (i.e., data written to @buf). */ - *n_transmittedp = pktlen; - return 0; -} - -/** - * packet_shutdown() - shutdown socket for future receive operations - * @sockfd: socket - * - * Partially emulates `shutdown(sockfd, SHUT_RD)`, in the sense that no - * further packets may be queued on the socket. All packets that are - * already queued will still be delivered, but once -EAGAIN is returned - * we are guaranteed never to be able to read more packets in the future. - * - * Return: 0 on success, or a negative error code on failure. - */ -int packet_shutdown(int sockfd) { - struct sock_filter filter[] = { - BPF_STMT(BPF_RET + BPF_K, 0), /* discard all packets */ - }; - struct sock_fprog fprog = { - .filter = filter, - .len = sizeof(filter) / sizeof(filter[0]), - }; - int r; - - r = setsockopt(sockfd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog)); - if (r < 0) - return -errno; - - return 0; -} |