diff --git a/icmp.c b/icmp.c index 6493ea9..233acf9 100644 --- a/icmp.c +++ b/icmp.c @@ -124,7 +124,8 @@ void icmp_sock_handler(const struct ctx *c, union epoll_ref ref, icmp_id_map[V4][id].seq = seq; } - tap_ip4_send(c, sr4->sin_addr.s_addr, IPPROTO_ICMP, buf, n); + tap_icmp4_send(c, sr4->sin_addr.s_addr, tap_ip4_daddr(c), + buf, n); } } diff --git a/tap.c b/tap.c index 0031d82..d250a0b 100644 --- a/tap.c +++ b/tap.c @@ -132,19 +132,19 @@ static void *tap_push_l2h(const struct ctx *c, void *buf, uint16_t proto) } /** - * tap_ip4_send() - Send IPv4 packet, with L2 headers, calculating L3/L4 checksums + * tap_push_ip4h() - Build IPv4 header for inbound packet, with checksum * @c: Execution context - * @src: IPv4 source address - * @proto: L4 protocol number - * @in: Payload + * @src: IPv4 source address, network order + * @dst: IPv4 destination address, network order * @len: L4 payload length + * @proto: L4 protocol number + * + * Return: pointer at which to write the packet's payload */ -void tap_ip4_send(const struct ctx *c, in_addr_t src, uint8_t proto, - const char *in, size_t len) +static void *tap_push_ip4h(char *buf, in_addr_t src, in_addr_t dst, + size_t len, uint8_t proto) { - char buf[USHRT_MAX]; - struct iphdr *ip4h = (struct iphdr *)tap_push_l2h(c, buf, ETH_P_IP); - char *data = (char *)(ip4h + 1); + struct iphdr *ip4h = (struct iphdr *)buf; ip4h->version = 4; ip4h->ihl = sizeof(struct iphdr) / 4; @@ -155,20 +155,61 @@ void tap_ip4_send(const struct ctx *c, in_addr_t src, uint8_t proto, ip4h->ttl = 255; ip4h->protocol = proto; ip4h->saddr = src; - ip4h->daddr = tap_ip4_daddr(c); + ip4h->daddr = dst; csum_ip4_header(ip4h); + return ip4h + 1; +} +/** + * tap_udp4_send() - Send UDP over IPv4 packet + * @c: Execution context + * @src: IPv4 source address + * @sport: UDP source port + * @dst: IPv4 destination address + * @dport: UDP destination port + * @in: UDP payload contents (not including UDP header) + * @len: UDP payload length (not including UDP header) + */ +/* cppcheck-suppress unusedFunction */ +void tap_udp4_send(const struct ctx *c, in_addr_t src, in_port_t sport, + in_addr_t dst, in_port_t dport, + const void *in, size_t len) +{ + size_t udplen = len + sizeof(struct udphdr); + char buf[USHRT_MAX]; + void *ip4h = tap_push_l2h(c, buf, ETH_P_IP); + void *uhp = tap_push_ip4h(ip4h, src, dst, udplen, IPPROTO_UDP); + struct udphdr *uh = (struct udphdr *)uhp; + char *data = (char *)(uh + 1); + + uh->source = htons(sport); + uh->dest = htons(dport); + uh->len = htons(udplen); + csum_udp4(uh, src, dst, in, len); memcpy(data, in, len); - if (ip4h->protocol == IPPROTO_UDP) { - struct udphdr *uh = (struct udphdr *)(ip4h + 1); + if (tap_send(c, buf, len + (data - buf)) < 0) + debug("tap: failed to send %lu bytes (IPv4)", len); +} - csum_udp4(uh, ip4h->saddr, ip4h->daddr, - uh + 1, len - sizeof(*uh)); - } else if (ip4h->protocol == IPPROTO_ICMP) { - struct icmphdr *ih = (struct icmphdr *)(ip4h + 1); - csum_icmp4(ih, ih + 1, len - sizeof(*ih)); - } +/** + * tap_icmp4_send() - Send ICMPv4 packet + * @c: Execution context + * @src: IPv4 source address + * @dst: IPv4 destination address + * @in: ICMP packet, including ICMP header + * @len: ICMP packet length, including ICMP header + */ +void tap_icmp4_send(const struct ctx *c, in_addr_t src, in_addr_t dst, + void *in, size_t len) +{ + char buf[USHRT_MAX]; + void *ip4h = tap_push_l2h(c, buf, ETH_P_IP); + char *data = tap_push_ip4h(ip4h, src, dst, len, IPPROTO_ICMP); + struct icmphdr *icmp4h = (struct icmphdr *)data; + + memcpy(data, in, len); + csum_icmp4(icmp4h, icmp4h + 1, len - sizeof(*icmp4h)); if (tap_send(c, buf, len + (data - buf)) < 0) debug("tap: failed to send %lu bytes (IPv4)", len); diff --git a/tap.h b/tap.h index d43c7a0..743bc58 100644 --- a/tap.h +++ b/tap.h @@ -7,10 +7,13 @@ #define TAP_H in_addr_t tap_ip4_daddr(const struct ctx *c); +void tap_udp4_send(const struct ctx *c, in_addr_t src, in_port_t sport, + in_addr_t dst, in_port_t dport, + const void *in, size_t len); +void tap_icmp4_send(const struct ctx *c, in_addr_t src, in_addr_t dst, + void *in, size_t len); const struct in6_addr *tap_ip6_daddr(const struct ctx *c, const struct in6_addr *src); -void tap_ip4_send(const struct ctx *c, in_addr_t src, uint8_t proto, - const char *in, size_t len); void tap_udp6_send(const struct ctx *c, const struct in6_addr *src, in_port_t sport, const struct in6_addr *dst, in_port_t dport,