1 files changed, 68 insertions, 68 deletions

diff --git a/icmp.c b/icmp.c
index 757ca61..40edd55 100644
--- a/icmp.c
+++ b/icmp.c
@@ -154,102 +154,102 @@ int icmp_tap_handler(const struct ctx *c, uint8_t pif, int af,
 		     const void *saddr, const void *daddr,
 		     const struct pool *p, const struct timespec *now)
 {
+	uint8_t proto = af == AF_INET ? IPPROTO_ICMP : IPPROTO_ICMPV6;
+	const char *const pname = af == AF_INET ? "ICMP" : "ICMPv6";
+	union {
+		struct sockaddr sa;
+		struct sockaddr_in sa4;
+		struct sockaddr_in6 sa6;
+	} sa = { .sa.sa_family = af };
+	const socklen_t sl = af == AF_INET ? sizeof(sa.sa4) : sizeof(sa.sa6);
+	struct icmp_id_sock *id_sock;
+	uint16_t id, seq;
 	size_t plen;
+	void *pkt;
+	int s;
 
 	(void)saddr;
 	(void)pif;
 
 	if (af == AF_INET) {
-		struct sockaddr_in sa = {
-			.sin_family = AF_INET,
-		};
-		union icmp_epoll_ref iref;
 		const struct icmphdr *ih;
-		int id, s;
 
-		ih = packet_get(p, 0, 0, sizeof(*ih), &plen);
-		if (!ih)
+		if (!(pkt = packet_get(p, 0, 0, sizeof(*ih), &plen)))
 			return 1;
 
+		ih =  (struct icmphdr *)pkt;
+		plen += sizeof(*ih);
+
 		if (ih->type != ICMP_ECHO)
 			return 1;
 
-		iref.id = id = ntohs(ih->un.echo.id);
+		id = ntohs(ih->un.echo.id);
+		id_sock = &icmp_id_map[V4][id];
+		seq = ntohs(ih->un.echo.sequence);
+		sa.sa4.sin_addr = *(struct in_addr *)daddr;
+	} else if (af == AF_INET6) {
+		const struct icmp6hdr *ih;
 
-		if ((s = icmp_id_map[V4][id].sock) < 0) {
-			s = sock_l4(c, AF_INET, IPPROTO_ICMP, &c->ip4.addr_out,
-				    c->ip4.ifname_out, 0, iref.u32);
-			if (s < 0)
-				goto fail_sock;
-			if (s > FD_REF_MAX) {
-				close(s);
-				return 1;
-			}
+		if (!(pkt = packet_get(p, 0, 0, sizeof(*ih), &plen)))
+			return 1;
 
-			icmp_id_map[V4][id].sock = s;
+		ih = (struct icmp6hdr *)pkt;
+		plen += sizeof(*ih);
 
-			debug("ICMP: new socket %i for echo ID %i", s, id);
-		}
-		icmp_id_map[V4][id].ts = now->tv_sec;
+		if (ih->icmp6_type != ICMPV6_ECHO_REQUEST)
+			return 1;
+
+		id = ntohs(ih->icmp6_identifier);
+		id_sock = &icmp_id_map[V6][id];
+		seq = ntohs(ih->icmp6_sequence);
+		sa.sa6.sin6_addr = *(struct in6_addr *)daddr;
+		sa.sa6.sin6_scope_id = c->ifi6;
+	} else {
+		ASSERT(0);
+	}
 
-		sa.sin_addr = *(struct in_addr *)daddr;
-		if (sendto(s, ih, sizeof(*ih) + plen, MSG_NOSIGNAL,
-			   (struct sockaddr *)&sa, sizeof(sa)) < 0) {
-			debug("ICMP: failed to relay request to socket");
+	if ((s = id_sock->sock) < 0) {
+		union icmp_epoll_ref iref = { .id = id };
+		const void *bind_addr;
+		const char *bind_if;
+
+		if (af == AF_INET) {
+			bind_addr = &c->ip4.addr_out;
+			bind_if = c->ip4.ifname_out;
 		} else {
-			debug("ICMP: echo request to socket, ID: %i, seq: %i",
-			      id, ntohs(ih->un.echo.sequence));
+			bind_addr = &c->ip6.addr_out;
+			bind_if = c->ip6.ifname_out;
 		}
-	} else if (af == AF_INET6) {
-		struct sockaddr_in6 sa = {
-			.sin6_family = AF_INET6,
-			.sin6_scope_id = c->ifi6,
-		};
-		union icmp_epoll_ref iref;
-		const struct icmp6hdr *ih;
-		int id, s;
 
-		ih = packet_get(p, 0, 0, sizeof(struct icmp6hdr), &plen);
-		if (!ih)
-			return 1;
+		s = sock_l4(c, af, proto, bind_addr, bind_if, 0, iref.u32);
 
-		if (ih->icmp6_type != ICMPV6_ECHO_REQUEST)
+		if (s < 0) {
+			warn("Cannot open \"ping\" socket. You might need to:");
+			warn("  sysctl -w net.ipv4.ping_group_range=\"0 2147483647\"");
+			warn("...echo requests/replies will fail.");
 			return 1;
-
-		iref.id = id = ntohs(ih->icmp6_identifier);
-		if ((s = icmp_id_map[V6][id].sock) < 0) {
-			s = sock_l4(c, AF_INET6, IPPROTO_ICMPV6,
-				    &c->ip6.addr_out,
-				    c->ip6.ifname_out, 0, iref.u32);
-			if (s < 0)
-				goto fail_sock;
-			if (s > FD_REF_MAX) {
-				close(s);
-				return 1;
-			}
-
-			icmp_id_map[V6][id].sock = s;
-
-			debug("ICMPv6: new socket %i for echo ID %i", s, id);
 		}
-		icmp_id_map[V6][id].ts = now->tv_sec;
 
-		sa.sin6_addr = *(struct in6_addr *)daddr;
-		if (sendto(s, ih, sizeof(*ih) + plen, MSG_NOSIGNAL,
-			   (struct sockaddr *)&sa, sizeof(sa)) < 1) {
-			debug("ICMPv6: failed to relay request to socket");
-		} else {
-			debug("ICMPv6: echo request to socket, ID: %i, seq: %i",
-			      id, ntohs(ih->icmp6_sequence));
+		if (s > FD_REF_MAX) {
+			close(s);
+			return 1;
 		}
+
+		id_sock->sock = s;
+
+		debug("%s: new socket %i for echo ID %"PRIu16, pname, s, id);
 	}
 
-	return 1;
+	id_sock->ts = now->tv_sec;
+
+	if (sendto(s, pkt, plen, MSG_NOSIGNAL, &sa.sa, sl) < 0) {
+		debug("%s: failed to relay request to socket: %s",
+		      pname, strerror(errno));
+	} else {
+		debug("%s: echo request to socket, ID: %"PRIu16", seq: %"PRIu16,
+		      pname, id, seq);
+	}
 
-fail_sock:
-	warn("Cannot open \"ping\" socket. You might need to:");
-	warn("  sysctl -w net.ipv4.ping_group_range=\"0 2147483647\"");
-	warn("...echo requests/replies will fail.");
 	return 1;
 }