2 files changed, 62 insertions, 2 deletions

diff --git a/src/libstrongswan/selectors/traffic_selector.c b/src/libstrongswan/selectors/traffic_selector.c
index 12f160224..00c78a91b 100644
--- a/src/libstrongswan/selectors/traffic_selector.c
+++ b/src/libstrongswan/selectors/traffic_selector.c
@@ -199,6 +199,14 @@ static int print_icmp(printf_hook_data_t *data, uint16_t port)
 }
 
 /**
+ * Print GRE key
+ */
+static int print_grekey(printf_hook_data_t *data, uint16_t from_port, uint16_t to_port)
+{
+	return print_in_hook(data, "%d", traffic_selector_grekey(from_port, to_port));
+}
+
+/**
  * Described in header.
  */
 int traffic_selector_printf_hook(printf_hook_data_t *data,
@@ -302,7 +310,11 @@ int traffic_selector_printf_hook(printf_hook_data_t *data,
 	/* build port string */
 	if (has_ports)
 	{
-		if (this->from_port == this->to_port)
+		if (this->protocol == IPPROTO_GRE)
+		{
+			written += print_grekey(data, this->from_port, this->to_port);
+		}
+		else if (this->from_port == this->to_port)
 		{
 			struct servent *serv;
 
@@ -376,7 +388,24 @@ METHOD(traffic_selector_t, get_subset, traffic_selector_t*,
 	/* select protocol, which is not zero */
 	protocol = max(this->protocol, other->protocol);
 
-	if ((is_opaque(this) && is_opaque(other)) ||
+	if (this->protocol == IPPROTO_GRE)
+	{
+		if (is_any(this))
+		{
+			from_port = other->from_port;
+			to_port = other->to_port;
+		}
+		else if (is_any(other) ||
+			 (this->from_port == other->from_port &&
+			  this->to_port == other->to_port))
+		{
+			from_port = this->from_port;
+			to_port = this->to_port;
+		}
+		else
+			return NULL;
+	}
+	else if ((is_opaque(this) && is_opaque(other)) ||
 		(is_opaque(this) && is_any(other)) ||
 		(is_opaque(other) && is_any(this)))
 	{
diff --git a/src/libstrongswan/selectors/traffic_selector.h b/src/libstrongswan/selectors/traffic_selector.h
index a9f78303c..5b08e708c 100644
--- a/src/libstrongswan/selectors/traffic_selector.h
+++ b/src/libstrongswan/selectors/traffic_selector.h
@@ -120,6 +120,9 @@ struct traffic_selector_t {
 	 * 8 bits and the code in the least significant 8 bits.  Use the utility
 	 * functions to extract them.
 	 *
+	 * If the protocol is GRE, the high 16-bits of the 32-bit GRE key is stored
+	 * in the from port. Use the utility function to merge and split them.
+	 *
 	 * @return			port
 	 */
 	uint16_t (*get_from_port)(traffic_selector_t *this);
@@ -134,6 +137,9 @@ struct traffic_selector_t {
 	 * 8 bits and the code in the least significant 8 bits.  Use the utility
 	 * functions to extract them.
 	 *
+	 * If the protocol is GRE, the low 16-bits of the 32-bit GRE key is stored
+	 * in the to port. Use the utility function to merge and split them.
+	 *
 	 * @return			port
 	 */
 	uint16_t (*get_to_port)(traffic_selector_t *this);
@@ -278,6 +284,31 @@ int traffic_selector_cmp(traffic_selector_t *a, traffic_selector_t *b,
 						 void *opts);
 
 /**
+ * Reconstruct the 32-bit GRE KEY in host order from a from/to ports.
+ *
+ * @param from_port			port number in host order
+ * @param to_port			port number in host order
+ * @return				GRE KEY in host order
+ */
+static inline uint32_t traffic_selector_grekey(uint16_t from_port, uint16_t to_port)
+{
+	return (from_port << 16) | to_port;
+}
+
+/**
+ * Split 32-bit GRE KEY in host order to from/to ports.
+ *
+ * @param grekey			grekey in host order
+ * @param from_port			from port in host order
+ * @param to_port			to port in host order
+ */
+static inline void traffic_selector_split_grekey(uint32_t grekey, uint16_t *from_port, uint16_t *to_port)
+{
+	*from_port = grekey >> 16;
+	*to_port = grekey & 0xffff;
+}
+
+/**
  * Create a new traffic selector using human readable params.
  *
  * If protocol is ICMP or ICMPv6 the ports are interpreted as follows:  If they