ip_packet: Allow creation of IP packets from data

diff --git a/src/libipsec/ip_packet.c b/src/libipsec/ip_packet.c
index 3c2069ae7ee561a9719b51ae40744306603e9c95..a846717efa2c8974fdd5e9f3a92948872cc1ad56 100644 (file)
--- a/src/libipsec/ip_packet.c
+++ b/src/libipsec/ip_packet.c
@@ -254,3 +254,154 @@ failed:
        chunk_free(&packet);
        return NULL;
 }
+
+/**
+ * Calculate the checksum for the pseudo IP header
+ */
+static u_int16_t pseudo_header_checksum(host_t *src, host_t *dst,
+                                                                               u_int8_t proto, chunk_t payload)
+{
+       switch (src->get_family(src))
+       {
+               case AF_INET:
+               {
+                       struct __attribute__((packed)) {
+                               u_int32_t src;
+                               u_int32_t dst;
+                               u_char zero;
+                               u_char proto;
+                               u_int16_t len;
+                       } pseudo = {
+                               .proto = proto,
+                               .len = htons(payload.len),
+                       };
+                       memcpy(&pseudo.src, src->get_address(src).ptr,
+                                  sizeof(pseudo.src));
+                       memcpy(&pseudo.dst, dst->get_address(dst).ptr,
+                                  sizeof(pseudo.dst));
+                       return chunk_internet_checksum(chunk_from_thing(pseudo));
+               }
+               case AF_INET6:
+               {
+                       struct __attribute__((packed)) {
+                               u_char src[16];
+                               u_char dst[16];
+                               u_int32_t len;
+                               u_char zero[3];
+                               u_char next_header;
+                       } pseudo = {
+                               .next_header = proto,
+                               .len = htons(payload.len),
+                       };
+                       memcpy(&pseudo.src, src->get_address(src).ptr,
+                                  sizeof(pseudo.src));
+                       memcpy(&pseudo.dst, dst->get_address(dst).ptr,
+                                  sizeof(pseudo.dst));
+                       return chunk_internet_checksum(chunk_from_thing(pseudo));
+               }
+       }
+       return 0xffff;
+}
+
+/**
+ * Calculate transport header checksums
+ */
+static void fix_transport_checksum(host_t *src, host_t *dst, u_int8_t proto,
+                                                                  chunk_t payload)
+{
+       u_int16_t sum = 0;
+
+       switch (proto)
+       {
+               case IPPROTO_UDP:
+               {
+                       struct udphdr *udp;
+
+                       if (payload.len < sizeof(*udp))
+                       {
+                               return;
+                       }
+                       udp = (struct udphdr*)payload.ptr;
+                       udp->check = 0;
+                       sum = pseudo_header_checksum(src, dst, proto, payload);
+                       udp->check = chunk_internet_checksum_inc(payload, sum);
+                       break;
+               }
+               case IPPROTO_TCP:
+               {
+                       struct tcphdr *tcp;
+
+                       if (payload.len < sizeof(*tcp))
+                       {
+                               return;
+                       }
+                       tcp = (struct tcphdr*)payload.ptr;
+                       tcp->check = 0;
+                       sum = pseudo_header_checksum(src, dst, proto, payload);
+                       tcp->check = chunk_internet_checksum_inc(payload, sum);
+                       break;
+               }
+               default:
+                       break;
+       }
+}
+
+/**
+ * Described in header.
+ */
+ip_packet_t *ip_packet_create_from_data(host_t *src, host_t *dst,
+                                                                               u_int8_t next_header, chunk_t data)
+{
+       chunk_t packet;
+       int family;
+
+       family = src->get_family(src);
+       if (family != dst->get_family(dst))
+       {
+               DBG1(DBG_ESP, "address family does not match");
+               return NULL;
+       }
+
+       switch (family)
+       {
+               case AF_INET:
+               {
+                       struct ip ip = {
+                               .ip_v = 4,
+                               .ip_hl = 5,
+                               .ip_len = htons(20 + data.len),
+                               .ip_ttl = 0x80,
+                               .ip_p = next_header,
+                       };
+                       memcpy(&ip.ip_src, src->get_address(src).ptr, sizeof(ip.ip_src));
+                       memcpy(&ip.ip_dst, dst->get_address(dst).ptr, sizeof(ip.ip_dst));
+                       ip.ip_sum = chunk_internet_checksum(chunk_from_thing(ip));
+
+                       packet = chunk_cat("cc", chunk_from_thing(ip), data);
+                       fix_transport_checksum(src, dst, next_header,
+                                                                  chunk_skip(packet, 20));
+                       return ip_packet_create(packet);
+               }
+#ifdef HAVE_NETINET_IP6_H
+               case AF_INET6:
+               {
+                       struct ip6_hdr ip = {
+                               .ip6_flow = htonl(6),
+                               .ip6_plen = htons(40 + data.len),
+                               .ip6_nxt = next_header,
+                               .ip6_hlim = 0x80,
+                       };
+                       memcpy(&ip.ip6_src, src->get_address(src).ptr, sizeof(ip.ip6_src));
+                       memcpy(&ip.ip6_dst, dst->get_address(dst).ptr, sizeof(ip.ip6_dst));
+
+                       packet = chunk_cat("cc", chunk_from_thing(ip), data);
+                       fix_transport_checksum(src, dst, next_header,
+                                                                  chunk_skip(packet, 40));
+                       return ip_packet_create(packet);
+               }
+#endif /* HAVE_NETINET_IP6_H */
+               default:
+                       DBG1(DBG_ESP, "unsupported address family");
+                       return NULL;
+       }
+}

diff --git a/src/libipsec/ip_packet.h b/src/libipsec/ip_packet.h
index de817e23eda862479aa0e1af846b8ed05791358e..0f44c895170954ddf8023da3014b06790b0a5799 100644 (file)
--- a/src/libipsec/ip_packet.h
+++ b/src/libipsec/ip_packet.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Tobias Brunner
+ * Copyright (C) 2012-2014 Tobias Brunner
  * Hochschule fuer Technik Rapperswil
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -93,4 +93,16 @@ struct ip_packet_t {
  */
 ip_packet_t *ip_packet_create(chunk_t packet);
 
+/**
+ * Encode an IP packet from the given data.
+ *
+ * @param src                  source address (cloned)
+ * @param dst                  destination address (cloned)
+ * @param next_header  the protocol (IPv4) or next header (IPv6)
+ * @param data                 complete data after basic IP header (cloned)
+ * @return                             ip_packet_t instance, or NULL if invalid
+ */
+ip_packet_t *ip_packet_create_from_data(host_t *src, host_t *dst,
+                                                                               u_int8_t next_header, chunk_t data);
+
 #endif /** IP_PACKET_H_ @}*/