Merge pull request #96 from pzanna/Dev_86

Dev 86

ZodiacFX/src/config/config_zodiac.h

@@ -31,14 +31,14 @@
 #define CONFIG_ZODIAC_H_
 
 
-#define VERSION "0.85"		// Firmware version number
+#define VERSION "0.86"		// Firmware version number
 
 #define TOTAL_PORTS 4		// Total number of physical ports on the Zodiac FX
 
 #define MAX_OFP_VERSION   0x04
 
 #define MAX_FLOWS_10	128		// Maximum number of flows for OpenFlow 1.0
-#define MAX_FLOWS_13	512		// Maximum number of flows for OpenFlow 1.3
+#define MAX_FLOWS_13	480		// Maximum number of flows for OpenFlow 1.3
 
 #define MAX_VLANS	4	// Maximum number of VLANS, default is 1 per port (4)
 

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;
 }
 
@@ -578,7 +587,7 @@ int flowmatch13(uint8_t *pBuffer, int port, uint8_t table_id, struct packet_fiel
 				break;
 
 				case OXM_OF_MPLS_LABEL:
-				if (fields->isMPLSTag && fields->mpls_label != *(uint32_t*)oxm_value)
+				if (!(fields->isMPLSTag && ntohl(fields->mpls_label) == *(uint32_t*)oxm_value))
 				{
 					priority_match = -1;
 				}
@@ -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;

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;

ZodiacFX/src/openflow/openflow.c

@@ -1,4 +1,6 @@
 /**
+ * vi: ts=4:sw=4
+ *
  * @file
  * openflow.c
  *
@@ -73,15 +75,20 @@ int tcp_con_state = -1;
 int tcp_wait = 0;
 int totaltime = 0;
 int heartbeat = 0;
-int multi_pos;
 uint32_t reply_more_xid = 0;
 bool reply_more_flag = false;
 bool rcv_freq;
 
+// Buffer for multi-segment messages
+#define PACKET_BUFFER_SIZE		(2*TCP_MSS+64)	// TODO: Ideally would be (2*1536)
+static uint8_t packet_buffer[PACKET_BUFFER_SIZE];
+static unsigned int packet_buffer_off = 0;
+static unsigned int packet_buffer_len = 0;
+
 // Internal Functions
 void OF_hello(void);
 void echo_request(void);
-void echo_reply(struct ofp_header *ofph, int size, int len);
+void echo_reply(struct ofp_header *ofph, int len);
 err_t TCPready(void *arg, struct tcp_pcb *tpcb, err_t err);
 void tcp_error(void * arg, err_t err);
 static err_t of_receive(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err);
@@ -133,43 +140,44 @@ void nnOF_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port)
 */
 static err_t of_receive(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
 {
-	char *pc;
-	char packetbuffer[1536];
 	heartbeat = 0;	// Reset heartbeat counter
 
 	if (err == ERR_OK && p != NULL)
 	{
-		tcp_recved(tpcb, p->tot_len);
-        pc=(char *)p->payload;	//pointer to the payload
-        int len = p->tot_len;	//size of the payload
-        for (int i=0; i<len; i++)packetbuffer[i] = pc[i];	//copy to our own buffer
-        pbuf_free(p);	//Free the packet buffer
-		TRACE("openflow.c: OpenFlow data received (%d bytes)", len);
-		struct ofp_header *ofph;
-		int size = 0;
-		int plen = 0;
-
-		while (size < len)
+		int plen = p->tot_len;	//size of the payload
+		if (packet_buffer_len + plen > PACKET_BUFFER_SIZE) {
+			TRACE("openflow.c: packet buffer exceeded, aborting!!!");
+			return ERR_ABRT;
+		}
+		memcpy(&packet_buffer[packet_buffer_len], p->payload, plen);	// append to our own buffer
+		packet_buffer_len += plen;
+		TRACE("openflow.c: OpenFlow data received (%d bytes)", plen);
+
+		// Accept the TCP data and release the packet
+		tcp_recved(tpcb, plen);
+		pbuf_free(p);
+
+		while (packet_buffer_off < packet_buffer_len)
 		{
-			ofph = &packetbuffer[size];
-			if (size == 0) multi_pos = 0;
+			struct ofp_header *ofph = &packet_buffer[packet_buffer_off];
 			if (ofph->length == 0 || ofph->version == 0){
-				return ERR_OK;	//Not an OpenFlow packet
+				TRACE("openflow.c: Invalid OpenFlow packet, aborting!");
+				return ERR_ABRT;
 			}
-			plen = htons(ofph->length);
-
-			if (ofph->version > 6 || ofph->type > 30) //	Invalid OpenFlow message
+			if (ofph->version > 6 || ofph->type > 30)
 			{
-				TRACE("openflow.c: Invalid OpenFlow command, ignoring!");
-				return ERR_OK;
+				TRACE("openflow.c: Invalid OpenFlow packet values, aborting!");
+				return ERR_ABRT;
 			}
-			size = size + plen;
-			if (size > len) // corrupt OpenFlow command
+
+			int ofp_len = htons(ofph->length);
+			if ((packet_buffer_off + ofp_len) > packet_buffer_len)
 			{
+				// TRACE("openflow.c: Partial OpenFlow message - waiting for more data...");
 				break;
-				TRACE("openflow.c: Corrupt OpenFlow Message!!!");	
 			}
-			TRACE("openflow.c: Processing %d byte OpenFlow message %u (%d)", plen, htonl(ofph->xid), size);
+			packet_buffer_off += ofp_len;
+			TRACE("openflow.c: Processing %d byte OpenFlow message %u", ofp_len, htonl(ofph->xid));
 
 			switch(ofph->type)
 			{
@@ -190,14 +198,24 @@ static err_t of_receive(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t e
 				break;
 
 				case OFPT10_ECHO_REQUEST:
-					echo_reply(ofph, size, len);
+					echo_reply(ofph, ofp_len);
 				break;
 
 				default:
-					if (OF_Version == 0x01) of10_message(ofph, size, len);
-					if (OF_Version == 0x04) of13_message(ofph, size, len);
+					if (OF_Version == 0x01) of10_message(ofph, ofp_len);
+					if (OF_Version == 0x04) of13_message(ofph, ofp_len);
 			};
+		}
 
+		if (packet_buffer_off == packet_buffer_len) {
+			packet_buffer_off = 0;
+			packet_buffer_len = 0;
+		} else {
+			unsigned int rem = packet_buffer_len - packet_buffer_off;
+			memcpy(packet_buffer, &packet_buffer[packet_buffer_off], rem);
+			packet_buffer_off = 0;
+			packet_buffer_len = rem;
+			TRACE("openflow.c: Partial OpenFlow message - keeping %d bytes", rem);
 		}
 	} else {
 		pbuf_free(p);
@@ -259,7 +277,7 @@ void OF_hello(void)
 *	@param xid - transaction ID
 *
 */
-void echo_reply(struct ofp_header *ofph, int size, int len)
+void echo_reply(struct ofp_header *ofph, int len)
 {
 	// Change the message type to Echo Reply and return any data that was sent
 	ofph->type   = OFPT10_ECHO_REPLY;
@@ -405,6 +423,8 @@ void task_openflow(void)
 */
 err_t TCPready(void *arg, struct tcp_pcb *tpcb, err_t err)
 {
+	packet_buffer_off = 0;
+	packet_buffer_len = 0;
 	tcp_con_state = true;
 	tcp_recv(tpcb, of_receive);
 	tcp_poll(tpcb, NULL, 4);

ZodiacFX/src/openflow/openflow.h

@@ -134,8 +134,8 @@ void task_openflow(void);
 void nnOF_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port);
 void nnOF10_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port);
 void nnOF13_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port);
-void of10_message(struct ofp_header *ofph, int size, int len);
-void of13_message(struct ofp_header *ofph, int size, int len);
+void of10_message(struct ofp_header *ofph, int len);
+void of13_message(struct ofp_header *ofph, int len);
 void multi_flow_more_reply13(void);
 void barrier10_reply(uint32_t xid);
 void barrier13_reply(uint32_t xid);

ZodiacFX/src/openflow/openflow_10.c

@@ -339,7 +339,7 @@ void nnOF10_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port)
 	return;	// Should only get to here if the action is unknown
 }
 
-void of10_message(struct ofp_header *ofph, int size, int len)
+void of10_message(struct ofp_header *ofph, int len)
 {
 	struct ofp_stats_request *stats_req;
 	switch(ofph->type)