From 0f3f75638d71e8bfffd17e2c3816f2c96322ae7d Mon Sep 17 00:00:00 2001
From: Paul Zanna <paul@northboundnetworks.com>
Date: Fri, 19 Jul 2019 18:43:55 +1000
Subject: [PATCH] Added ARP matching

---
 ZodiacFX/src/openflow/of_helper.c            | 131 +++++++++++++++++++
 ZodiacFX/src/openflow/of_helper.h            |   5 +
 ZodiacFX/src/openflow_spec/openflow_spec13.h |   2 +
 ZodiacFX/src/switch.c                        |   2 +-
 4 files changed, 139 insertions(+), 1 deletion(-)

diff --git a/ZodiacFX/src/openflow/of_helper.c b/ZodiacFX/src/openflow/of_helper.c
index 9976c6d..b24aa09 100644
--- a/ZodiacFX/src/openflow/of_helper.c
+++ b/ZodiacFX/src/openflow/of_helper.c
@@ -336,6 +336,15 @@ void packet_fields_parser(uint8_t *pBuffer, struct packet_fields *fields) {
 			fields->tp_dst = udphdr->dest;
 		}
 	}
+	
+	if(ntohs(fields->eth_prot) == 0x0806){
+		memcpy(&fields->arp_op, fields->payload + 6, 2);
+		memcpy(&fields->arp_sha, fields->payload + 8, 6);
+		memcpy(&fields->arp_spa, fields->payload + 14, 4);
+		memcpy(&fields->arp_tha, fields->payload + 18, 6);
+		memcpy(&fields->arp_tpa, fields->payload + 24, 4);
+	}
+	
 	fields->parsed = true;
 }
 
@@ -597,7 +606,129 @@ int flowmatch13(uint8_t *pBuffer, int port, uint8_t table_id, struct packet_fiel
 					priority_match = -1;
 				}
 				break;
+
+				case OXM_OF_ARP_OP:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					if (fields->arp_op != *(uint16_t*)oxm_value)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+				
+				case OXM_OF_ARP_SPA:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					if (memcmp(&fields->arp_spa, oxm_value, 4) != 0)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+
+				case OXM_OF_ARP_SPA_W:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					memcpy(oxm_ipv4, &fields->arp_spa, 4);
+					for (int j=0; j<4; j++)
+					{
+						oxm_ipv4[j] &= oxm_value[4+j];
+					}
+					if (memcmp(oxm_ipv4, oxm_value, 4) != 0)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+				
+				case OXM_OF_ARP_TPA:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					if (memcmp(&fields->arp_tpa, oxm_value, 4) != 0)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+
+				case OXM_OF_ARP_TPA_W:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					memcpy(oxm_ipv4, &fields->arp_tpa, 4);
+					for (int j=0; j<4; j++)
+					{
+						oxm_ipv4[j] &= oxm_value[4+j];
+					}
+					if (memcmp(oxm_ipv4, oxm_value, 4) != 0)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+				
+				case OXM_OF_ARP_SHA:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					if (memcmp(&fields->arp_sha, oxm_value, 6) != 0)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+
+				case OXM_OF_ARP_SHA_W:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					for (int j=0; j<6; j++ )
+					{
+						if ((oxm_value[j] & oxm_value[6+j]) != fields->arp_sha[j] & oxm_value[6+j]){
+							priority_match = -1;
+						}
+					}
+				}
+				break;
 				
+				case OXM_OF_ARP_THA:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					if (memcmp(&fields->arp_tha, oxm_value, 6) != 0)
+					{
+						priority_match = -1;
+					}
+				}
+				break;
+
+				case OXM_OF_ARP_THA_W:
+				if (fields->eth_prot != htons(0x0806))
+				{
+					priority_match = -1;
+				} else {
+					for (int j=0; j<6; j++ )
+					{
+						if ((oxm_value[j] & oxm_value[6+j]) != fields->arp_tha[j] & oxm_value[6+j]){
+							priority_match = -1;
+						}
+					}
+				}
+				break;
+												
 				default:
 				priority_match = -1;
 				break;
diff --git a/ZodiacFX/src/openflow/of_helper.h b/ZodiacFX/src/openflow/of_helper.h
index 9dea30a..640a64b 100644
--- a/ZodiacFX/src/openflow/of_helper.h
+++ b/ZodiacFX/src/openflow/of_helper.h
@@ -44,6 +44,11 @@ struct packet_fields
 	uint16_t vlanid;
 	uint32_t ip_src;
 	uint32_t ip_dst;
+	uint32_t arp_spa;
+	uint32_t arp_tpa;
+	uint32_t arp_op;
+	uint8_t arp_sha[6];
+	uint8_t arp_tha[6];
 	uint32_t mpls_label;
 	uint8_t mpls_tc;
 	uint8_t mpls_bos;
diff --git a/ZodiacFX/src/openflow_spec/openflow_spec13.h b/ZodiacFX/src/openflow_spec/openflow_spec13.h
index cb76e8b..d1db5f1 100644
--- a/ZodiacFX/src/openflow_spec/openflow_spec13.h
+++ b/ZodiacFX/src/openflow_spec/openflow_spec13.h
@@ -1498,7 +1498,9 @@ enum ofp_vlan_id {
  *
  * Masking: Not maskable. */
 #define OXM_OF_ARP_SHA    OXM_HEADER  (0x8000, OFPXMT_OFB_ARP_SHA, 6)
+#define OXM_OF_ARP_SHA_W  OXM_HEADER_W(0x8000, OFPXMT_OFB_ARP_SHA, 6)
 #define OXM_OF_ARP_THA    OXM_HEADER  (0x8000, OFPXMT_OFB_ARP_THA, 6)
+#define OXM_OF_ARP_THA_W  OXM_HEADER_W(0x8000, OFPXMT_OFB_ARP_THA, 6)
 
 /* The source or destination address in the IPv6 header.
  *
diff --git a/ZodiacFX/src/switch.c b/ZodiacFX/src/switch.c
index 6454f41..e24c4cc 100644
--- a/ZodiacFX/src/switch.c
+++ b/ZodiacFX/src/switch.c
@@ -436,7 +436,7 @@ void switch_init(void)
 				uint8_t vlanmaplow;
 				vlanmaphigh = 16; // Set valid bit
 				if (Zodiac_Config.vlan_list[x].uVlanType == 2) vlanmaphigh += 8; // Port 5 (CPU);
-				switch_write(80,2);	// remove any VLAN tags going to the CPU
+				//switch_write(80,2);	// remove any VLAN tags going to the CPU
 				if (Zodiac_Config.vlan_list[x].portmap[3] == 1) // Port 4
 				{
 					vlanmaphigh += 4;