From 108a67893f0a32ecee44742c452964e03c0a6c63 Mon Sep 17 00:00:00 2001
From: Tobias Brunner <tobias@strongswan.org>
Date: Tue, 15 Jul 2014 17:19:48 +0200
Subject: [PATCH] ip_packet: Apply transport protocol ports when encoding IP
 packet

---
 src/libipsec/ip_packet.c | 33 +++++++++++++++++++++++++--------
 src/libipsec/ip_packet.h |  7 +++++--
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/src/libipsec/ip_packet.c b/src/libipsec/ip_packet.c
index 4ef8fd5b8e..7de4658b9e 100644
--- a/src/libipsec/ip_packet.c
+++ b/src/libipsec/ip_packet.c
@@ -317,12 +317,15 @@ static u_int16_t pseudo_header_checksum(host_t *src, host_t *dst,
 }
 
 /**
- * Calculate transport header checksums
+ * Apply transport ports and calculate header checksums
  */
-static void fix_transport_checksum(host_t *src, host_t *dst, u_int8_t proto,
-								   chunk_t payload)
+static void fix_transport_header(host_t *src, host_t *dst, u_int8_t proto,
+								 chunk_t payload)
 {
-	u_int16_t sum = 0;
+	u_int16_t sum = 0, sport, dport;
+
+	sport = src->get_port(src);
+	dport = dst->get_port(dst);
 
 	switch (proto)
 	{
@@ -335,6 +338,14 @@ static void fix_transport_checksum(host_t *src, host_t *dst, u_int8_t proto,
 				return;
 			}
 			udp = (struct udphdr*)payload.ptr;
+			if (sport != 0)
+			{
+				udp->source = htons(sport);
+			}
+			if (dport != 0)
+			{
+				udp->dest = htons(dport);
+			}
 			udp->check = 0;
 			sum = pseudo_header_checksum(src, dst, proto, payload);
 			udp->check = chunk_internet_checksum_inc(payload, sum);
@@ -349,6 +360,14 @@ static void fix_transport_checksum(host_t *src, host_t *dst, u_int8_t proto,
 				return;
 			}
 			tcp = (struct tcphdr*)payload.ptr;
+			if (sport != 0)
+			{
+				tcp->source = htons(sport);
+			}
+			if (dport != 0)
+			{
+				tcp->dest = htons(dport);
+			}
 			tcp->check = 0;
 			sum = pseudo_header_checksum(src, dst, proto, payload);
 			tcp->check = chunk_internet_checksum_inc(payload, sum);
@@ -391,8 +410,7 @@ ip_packet_t *ip_packet_create_from_data(host_t *src, host_t *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));
+			fix_transport_header(src, dst, next_header, chunk_skip(packet, 20));
 			return ip_packet_create(packet);
 		}
 #ifdef HAVE_NETINET_IP6_H
@@ -408,8 +426,7 @@ ip_packet_t *ip_packet_create_from_data(host_t *src, host_t *dst,
 			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));
+			fix_transport_header(src, dst, next_header, chunk_skip(packet, 40));
 			return ip_packet_create(packet);
 		}
 #endif /* HAVE_NETINET_IP6_H */
diff --git a/src/libipsec/ip_packet.h b/src/libipsec/ip_packet.h
index 7f7c1a314a..73721cfd1d 100644
--- a/src/libipsec/ip_packet.h
+++ b/src/libipsec/ip_packet.h
@@ -103,8 +103,11 @@ 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)
+ * If src and/or dst have ports set they are applied to UDP/TCP headers found
+ * in the packet.
+ *
+ * @param src			source address and optional port (cloned)
+ * @param dst			destination address and optional port (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
-- 
2.39.2