diff --git a/README.md b/README.md index 6f35202..78459f7 100644 --- a/README.md +++ b/README.md @@ -14,17 +14,17 @@ This repository contains the entire open-source firmware for the Zodiac FX inclu The latest firmware is available in the [Northbound Networks Forums](http://forums.northboundnetworks.com/index.php?topic=52.0). -Starting from version 0.80, Zodiac FX supports firmware updates via the CLI and web interface. In order to make this possible, major changes were made to the firmware flashing process. Follow the update process below, based on your current firmware version. +Starting from version 0.81, Zodiac FX supports firmware updates via the CLI and web interface. In order to make this possible, major changes were made to the firmware flashing process. Follow the update process below, based on your current firmware version. -#### For firmware versions BEFORE version 0.80 +#### For firmware versions BEFORE version 0.81 -To update to version 0.80 or later, a **full upgrade firmware** needs to be flashed. +To update to version 0.81 or later, a **full upgrade firmware** needs to be flashed. Download the latest **full upgrade firmware** from the [Northbound Networks Forums](http://forums.northboundnetworks.com/index.php?topic=52.0) - 'Full Upgrade Firmware (v0.XX) - ZodiacFX_vXX_full_install.bin' Follow the firmware update process detailed in **Section 2. Updating Firmware** in the [Zodiac FX User Guide](http://forums.northboundnetworks.com/downloads/zodiac_fx/guides/ZodiacFX_UserGuide_0317.pdf). -#### For firmware versions AFTER version 0.80 +#### For firmware versions AFTER version 0.81 The update process has been simplified for the newer releases. @@ -83,7 +83,7 @@ The Zodiac FX uses a simple additive checksum to verify the integrity of the upl To sign your own modified firmware, follow the steps below: * Build a 'Release' binary of the modified firmware * Update the Zodiac FX with the modified firmware - * Follow the instructions outlined in Flashing/Updating the Firmware - For firmware versions AFTER version 0.80 + * Follow the instructions outlined in Flashing/Updating the Firmware - For firmware versions AFTER version 0.81 * The firmware will fail the verification check, but will still be stored inside the Zodiac FX flash memory * In the root context of the CLI, type in the hidden command 'get crc' * Open the ZodiacFX.bin file in a hex editor, and append the 8 bytes to the end of the firmware file @@ -99,6 +99,12 @@ Issues can also be [raised](https://github.com/NorthboundNetworks/ZodiacFX/issue ## Release Notes +**Version 0.81** +* Firmware upload fixes (full upgrade required) +* Metering bug-fixes & updates (initial DSCP remark support) +* Port stat output bug-fixes +* Web interface improvements + **Version 0.80** * Firmware upload via CLI and web interface added * Metering added to OpenFlow 1.3 diff --git a/ZodiacFX/ZodiacFX.cproj b/ZodiacFX/ZodiacFX.cproj index e3e4036..bb1eeff 100644 --- a/ZodiacFX/ZodiacFX.cproj +++ b/ZodiacFX/ZodiacFX.cproj @@ -274,11 +274,11 @@ JTAG com.atmel.avrdbg.tool.atmelice - J41800058832 + J41800009874 Atmel-ICE JTAG - J41800058832 + J41800009874 0xA3CC0CE0 7500000 @@ -368,7 +368,7 @@ True - -Wl,--defsym,__stack_size__=0x1400 -Wl,--entry=Reset_Handler -Wl,-section-start=.text=0x00420000 -Wl,--cref -mthumb -T../src/ASF/sam/utils/linker_scripts/sam4e/sam4e8/gcc/flash.ld + -Wl,--defsym,__stack_size__=0x1400 -Wl,--entry=Reset_Handler -Wl,-section-start=.text=0x00410000 -Wl,--cref -mthumb -T../src/ASF/sam/utils/linker_scripts/sam4e/sam4e8/gcc/flash.ld ../src/ASF/sam/drivers/efc diff --git a/ZodiacFX/src/command.c b/ZodiacFX/src/command.c index 864f33d..8c1befa 100644 --- a/ZodiacFX/src/command.c +++ b/ZodiacFX/src/command.c @@ -24,6 +24,7 @@ * along with this program. If not, see . * * Author: Paul Zanna + * & Kristopher Chen * */ @@ -60,16 +61,14 @@ extern uint16_t ofp13_oxm_inst_size[MAX_FLOWS_13]; extern struct flows_counter flow_counters[MAX_FLOWS_13]; extern struct flow_tbl_actions *flow_actions10[MAX_FLOWS_13]; extern int iLastFlow; -extern struct ofp10_port_stats phys10_port_stats[4]; -extern struct ofp13_port_stats phys13_port_stats[4]; +extern struct ofp10_port_stats phys10_port_stats[TOTAL_PORTS]; +extern struct ofp13_port_stats phys13_port_stats[TOTAL_PORTS]; extern struct table_counter table_counters[MAX_TABLES]; extern struct meter_entry13 *meter_entry[MAX_METER_13]; extern struct meter_band_stats_array band_stats_array[MAX_METER_13]; -extern bool masterselect; -extern bool stackenabled = false; extern bool trace = false; extern struct tcp_pcb *tcp_pcb; -extern uint8_t port_status[4]; +extern uint8_t port_status[TOTAL_PORTS]; extern int totaltime; extern int32_t ul_temp; extern int OF_Version; @@ -82,7 +81,6 @@ uint8_t uCLIContext = 0; struct arp_header arp_test; uint8_t esc_char = 0; - // Internal Functions void saveConfig(void); void command_root(char *command, char *param1, char *param2, char *param3); @@ -124,6 +122,18 @@ void saveConfig(void) return; } +/* +* Restart Zodiac FX +* +*/ +void software_reset(void) +{ + for(int x = 0;x<100000;x++); // Let the above message get sent to the terminal before detaching + udc_detach(); // Detach the USB device before restart + rstc_start_software_reset(RSTC); // Software reset + while (1); +} + /* * Main command line loop * @@ -142,10 +152,7 @@ void task_command(char *str, char *str_last) if(restart_required_outer == true) { printf("Restarting the Zodiac FX, please reopen your terminal application.\r\n"); - for(int x = 0;x<100000;x++); // Let the above message get sent to the terminal before detaching - udc_detach(); // Detach the USB device before restart - rstc_start_software_reset(RSTC); // Software reset - while (1); + software_reset(); } while(udi_cdc_is_rx_ready()){ @@ -318,28 +325,35 @@ void command_root(char *command, char *param1, char *param2, char *param3) { int i; printf("\r\n-------------------------------------------------------------------------\r\n"); - for (i=0;i<4;i++) + for (i=0;i 3) { - if (Zodiac_Config.vlan_list[x].portmap[i] == 1) + printf(" VLAN type: OpenFlow\r\n"); + printf(" VLAN ID: n/a\r\n"); + } else + { + for (int x=0;x 3) printf(" RX Packets: %" PRIu64 "\r\n", phys10_port_stats[i].rx_packets); + if (Zodiac_Config.of_port[i] == 1 || i > 3) printf(" TX Packets: %" PRIu64 "\r\n", phys10_port_stats[i].tx_packets); printf(" RX Dropped Packets: %" PRIu64 "\r\n", phys10_port_stats[i].rx_dropped); printf(" TX Dropped Packets: %" PRIu64 "\r\n", phys10_port_stats[i].tx_dropped); printf(" RX CRC Errors: %" PRIu64 "\r\n", phys10_port_stats[i].rx_crc_err); @@ -348,8 +362,8 @@ void command_root(char *command, char *param1, char *param2, char *param3) { printf(" RX Bytes: %" PRIu64 "\r\n", phys13_port_stats[i].rx_bytes); printf(" TX Bytes: %" PRIu64 "\r\n", phys13_port_stats[i].tx_bytes); - if (Zodiac_Config.of_port[i] == 1) printf(" RX Packets: %" PRIu64 "\r\n", phys13_port_stats[i].rx_packets); - if (Zodiac_Config.of_port[i] == 1) printf(" TX Packets: %" PRIu64 "\r\n", phys13_port_stats[i].tx_packets); + if (Zodiac_Config.of_port[i] == 1 || i > 3) printf(" RX Packets: %" PRIu64 "\r\n", phys13_port_stats[i].rx_packets); + if (Zodiac_Config.of_port[i] == 1 || i > 3) printf(" TX Packets: %" PRIu64 "\r\n", phys13_port_stats[i].tx_packets); printf(" RX Dropped Packets: %" PRIu64 "\r\n", phys13_port_stats[i].rx_dropped); printf(" TX Dropped Packets: %" PRIu64 "\r\n", phys13_port_stats[i].tx_dropped); printf(" RX CRC Errors: %" PRIu64 "\r\n", phys13_port_stats[i].rx_crc_err); @@ -447,10 +461,7 @@ void command_root(char *command, char *param1, char *param2, char *param3) if (strcmp(command, "restart")==0) { printf("Restarting the Zodiac FX, please reopen your terminal application.\r\n"); - for(int x = 0;x<100000;x++); // Let the above message get sent to the terminal before detaching - udc_detach(); // Detach the USB device before restart - rstc_start_software_reset(RSTC); // Software reset - while (1); + software_reset(); } // Get CRC @@ -461,6 +472,19 @@ void command_root(char *command, char *param1, char *param2, char *param3) printf("Append [%08x 00000000] to the binary\r\n", ntohl(verify.calculated)); return; } + + if (strcmp(command, "dump")==0 && strcmp(param1, "flash")==0) + { + uint8_t* buffer_pmem = FLASH_BUFFER; + while(buffer_pmem < FLASH_BUFFER_END) + { + printf("%02x", *buffer_pmem); + buffer_pmem++; + } + printf("\n"); + + return; + } // Unknown Command printf("Unknown command\r\n"); @@ -506,10 +530,7 @@ void command_config(char *command, char *param1, char *param2, char *param3) if (strcmp(command, "restart")==0) { printf("Restarting the Zodiac FX, please reopen your terminal application.\r\n"); - for(int x = 0;x<100000;x++); // Let the above message get send to the terminal before detaching - udc_detach(); // Detach the USB device before restart - rstc_start_software_reset(RSTC); // Software reset - while (1); + software_reset(); } // Display Config @@ -534,10 +555,6 @@ void command_config(char *command, char *param1, char *param2, char *param3) } else { printf(" Force OpenFlow version: Disabled\r\n"); } - if (masterselect == true) printf(" Stacking Select: SLAVE\r\n"); - if (masterselect == false) printf(" Stacking Select: MASTER\r\n"); - if (stackenabled == true) printf(" Stacking Status: Enabled\r\n"); - if (stackenabled == false) printf(" Stacking Select: Disabled\r\n"); if (Zodiac_Config.ethtype_filter == 1) printf(" EtherType Filtering: Enabled\r\n"); if (Zodiac_Config.ethtype_filter != 1) printf(" EtherType Filtering: Disabled\r\n"); printf("\r\n-------------------------------------------------------------------------\r\n\n"); @@ -1540,7 +1557,7 @@ void command_openflow(char *command, char *param1, char *param2, char *param3) } else if(ptr_band->type == OFPMBT13_DSCP_REMARK) { - printf("DSCP REMARK (unsupported)\r\n"); + printf("DSCP REMARK\r\n"); } else { @@ -1549,6 +1566,11 @@ void command_openflow(char *command, char *param1, char *param2, char *param3) printf("\t\t Rate:\t\t%d\t\r\n", ptr_band->rate); printf("\t\t Burst Size:\t%d\t\r\n", ptr_band->burst_size); + if(ptr_band->type == OFPMBT13_DSCP_REMARK) + { + printf("\t\t Precedence:\t+%d\t\r\n", ((struct ofp13_meter_band_dscp_remark*)ptr_band)->prec_level); + } + // Find band index int band_index = ((uint8_t*)ptr_band - (uint8_t*)&(meter_entry[meter_index]->bands)) / sizeof(struct ofp13_meter_band_drop); @@ -1611,12 +1633,6 @@ void command_debug(char *command, char *param1, char *param2, char *param3) return; } - if (strcmp(command, "spi")==0) - { - //stack_write(atoi(param1)); - return; - } - if (strcmp(command, "mem")==0) { printf("mem total: %d\r\n", membag_get_total()); @@ -1670,10 +1686,12 @@ void printhelp(void) printf(" config\r\n"); printf(" openflow\r\n"); printf(" debug\r\n"); - printf(" show ports\r\n"); + printf(" update\r\n"); printf(" show status\r\n"); printf(" show version\r\n"); + printf(" show ports\r\n"); printf(" restart\r\n"); + printf(" help\r\n"); printf("\r\n"); printf("Config:\r\n"); printf(" save\r\n"); @@ -1693,13 +1711,14 @@ void printhelp(void) printf(" set vlan-type \r\n"); printf(" add vlan-port \r\n"); printf(" delete vlan-port \r\n"); - printf(" factory reset\r\n"); printf(" set of-version \r\n"); printf(" set ethertype-filter \r\n"); + printf(" factory reset\r\n"); printf(" exit\r\n"); printf("\r\n"); printf("OpenFlow:\r\n"); printf(" show status\r\n"); + printf(" show tables\r\n"); printf(" show flows\r\n"); printf(" show meters\r\n"); printf(" enable\r\n"); diff --git a/ZodiacFX/src/command.h b/ZodiacFX/src/command.h index b2d0f36..bee2417 100644 --- a/ZodiacFX/src/command.h +++ b/ZodiacFX/src/command.h @@ -93,5 +93,6 @@ typedef struct arp_header { void task_command(char *str, char * str_last); void loadConfig(void); +void software_reset(void); #endif /* COMMANDS_H_ */ diff --git a/ZodiacFX/src/config/config_zodiac.h b/ZodiacFX/src/config/config_zodiac.h index 6698768..bdfbe2c 100644 --- a/ZodiacFX/src/config/config_zodiac.h +++ b/ZodiacFX/src/config/config_zodiac.h @@ -31,7 +31,9 @@ #define CONFIG_ZODIAC_H_ -#define VERSION "0.80" // Firmware version number +#define VERSION "0.81" // Firmware version number + +#define TOTAL_PORTS 4 // Total number of physical ports on the Zodiac FX #define MAX_OFP_VERSION 0x04 diff --git a/ZodiacFX/src/flash.c b/ZodiacFX/src/flash.c index 25ac197..c02ac78 100644 --- a/ZodiacFX/src/flash.c +++ b/ZodiacFX/src/flash.c @@ -36,19 +36,18 @@ #include "openflow/openflow.h" #include "trace.h" #include "command.h" +#include "switch.h" // Global variables extern uint8_t shared_buffer[SHARED_BUFFER_LEN]; struct verification_data verify; +uint32_t flash_page_addr; // Static variables static uint32_t page_addr; //static uint32_t ul_rc; -static uint32_t flash_page_addr; static uint32_t ul_rc; -static uint32_t ul_idx; -static uint32_t ul_page_buffer[IFLASH_PAGE_SIZE / sizeof(uint32_t)]; /* @@ -67,7 +66,7 @@ void get_serial(uint32_t *uid_buf) */ int firmware_update_init(void) { - flash_page_addr = NEW_FW_BASE; + flash_page_addr = FLASH_BUFFER; /* Initialize flash: 6 wait states for flash writing. */ ul_rc = flash_init(FLASH_ACCESS_MODE_128, 6); @@ -92,19 +91,18 @@ int firmware_update_init(void) unlock_address += IFLASH_LOCK_REGION_SIZE; } - // Erase 192k + // Erase 32 pages at a time uint32_t erase_address = flash_page_addr; - while(erase_address < IFLASH_ADDR + IFLASH_SIZE - (ERASE_SECTOR_SIZE - 1)) + while(erase_address < FLASH_BUFFER_END) { - //printf("-I- Erasing sector with address: 0x%08x\r\n", erase_address); - ul_rc = flash_erase_sector(erase_address); + ul_rc = flash_erase_page(erase_address, IFLASH_ERASE_PAGES_32); if (ul_rc != FLASH_RC_OK) { //printf("-F- Flash programming error %lu\n\r", (unsigned long)ul_rc); return 0; } - erase_address += ERASE_SECTOR_SIZE; + erase_address += ((uint8_t)32*IFLASH_PAGE_SIZE); } return 1; @@ -152,9 +150,7 @@ void cli_update(void) if(verification_check() == SUCCESS) { printf("Firmware upload complete - Restarting the Zodiac FX.\r\n"); - for(int x = 0;x<100000;x++); // Let the above message get send to the terminal before detaching - udc_detach(); // Detach the USB device before restart - rstc_start_software_reset(RSTC); // Software reset + software_reset(); } else { @@ -343,7 +339,7 @@ int xmodem_xfer(void) * Remove XMODEM 0x1A padding at end of data * */ -xmodem_clear_padding(uint8_t *buff) +void xmodem_clear_padding(uint8_t *buff) { int len = IFLASH_PAGE_SIZE; @@ -372,4 +368,3 @@ xmodem_clear_padding(uint8_t *buff) return; // Padding characters removed } - diff --git a/ZodiacFX/src/flash.h b/ZodiacFX/src/flash.h index 7c25a9e..f08c547 100644 --- a/ZodiacFX/src/flash.h +++ b/ZodiacFX/src/flash.h @@ -52,11 +52,7 @@ struct verification_data #define X_ACK 0x06 #define X_NAK 0x15 -#define ERASE_SECTOR_SIZE 8192 -#define NEW_FW_BASE (IFLASH_ADDR + (5*IFLASH_NB_OF_PAGES/8)*IFLASH_PAGE_SIZE) -#define NEW_FW_MAX_SIZE 196608 - -#define FLASH_BUFFER 0x450000 +#define FLASH_BUFFER 0x448000 #define FLASH_BUFFER_END 0x480000 #define NN_VERIFICATION_LEN 8 diff --git a/ZodiacFX/src/http.c b/ZodiacFX/src/http.c index fca726f..9846d63 100644 --- a/ZodiacFX/src/http.c +++ b/ZodiacFX/src/http.c @@ -48,12 +48,13 @@ extern int totaltime; extern int32_t ul_temp; extern struct zodiac_config Zodiac_Config; -extern uint8_t port_status[4]; +extern uint8_t port_status[8]; extern uint32_t uid_buf[4]; // Unique identifier extern struct tcp_pcb *tcp_pcb; extern int OF_Version; extern uint8_t shared_buffer[SHARED_BUFFER_LEN]; // SHARED_BUFFER_LEN must never be reduced below 2048 extern int tcp_con_state; // Check connection state +extern uint32_t flash_page_addr; extern struct ofp_flow_mod *flow_match10[MAX_FLOWS_10]; extern struct ofp13_flow_mod *flow_match13[MAX_FLOWS_13]; @@ -66,13 +67,10 @@ extern struct meter_entry13 *meter_entry[MAX_METER_13]; extern struct meter_band_stats_array band_stats_array[MAX_METER_13]; extern int iLastFlow; extern int iLastMeter; -extern struct ofp10_port_stats phys10_port_stats[4]; -extern struct ofp13_port_stats phys13_port_stats[4]; +extern struct ofp10_port_stats phys10_port_stats[TOTAL_PORTS]; +extern struct ofp13_port_stats phys13_port_stats[TOTAL_PORTS]; extern struct table_counter table_counters[MAX_TABLES]; -extern int firmware_update_init(void); -extern int flash_write_page(uint8_t *flash_page); - // Local Variables struct tcp_pcb *http_pcb; static char http_msg[64]; // Buffer for HTTP message filtering @@ -98,7 +96,9 @@ void http_send(char *buffer, struct tcp_pcb *pcb, bool out); static err_t http_sent(void *arg, struct tcp_pcb *tpcb, uint16_t len); void http_close(struct tcp_pcb *pcb); -static uint8_t upload_handler(char *payload, int len); +static uint8_t upload_handler(char *payload, int len); +static uint8_t process_pagebuff(uint8_t * buff_addr, uint16_t buff_index, uint8_t * match_addr); +static uint16_t send_pagebuff(uint8_t * buff_addr, uint16_t buff_index); // HTML resources static uint8_t interfaceCreate_Frames(void); @@ -211,12 +211,9 @@ static err_t http_sent(void *arg, struct tcp_pcb *tpcb, uint16_t len) } if(restart_required == true) { + // Indicates to task_command() that a restart is required on the next loop + // This allows the 'Restarting...' page to display before the restart occurs restart_required_outer = true; - //TRACE("http.c: restarting the Zodiac FX. Please reconnect."); - //for(int x = 0;x<100000;x++); // Let the above message get sent to the terminal before detaching - //udc_detach(); // Detach the USB device before restart - //rstc_start_software_reset(RSTC); // Software reset - //while (1); } return ERR_OK; @@ -341,14 +338,30 @@ static err_t http_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err else { upload_handler(NULL, 0); // Clean up upload operation - if(interfaceCreate_Upload_Status(3)) + + if(flash_page_addr >= FLASH_BUFFER_END-IFLASH_PAGE_SIZE) { - http_send(&shared_buffer, pcb, 1); - TRACE("http.c: Page sent successfully - %d bytes", strlen(shared_buffer)); + if(interfaceCreate_Upload_Status(5)) + { + http_send(&shared_buffer, pcb, 1); + TRACE("http.c: Page sent successfully - %d bytes", strlen(shared_buffer)); + } + else + { + TRACE("http.c: Unable to serve page - buffer at %d bytes", strlen(shared_buffer)); + } } else { - TRACE("http.c: Unable to serve page - buffer at %d bytes", strlen(shared_buffer)); + if(interfaceCreate_Upload_Status(3)) + { + http_send(&shared_buffer, pcb, 1); + TRACE("http.c: Page sent successfully - %d bytes", strlen(shared_buffer)); + } + else + { + TRACE("http.c: Unable to serve page - buffer at %d bytes", strlen(shared_buffer)); + } } } } @@ -472,7 +485,24 @@ static err_t http_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err if(interfaceCreate_Display_Ports(1)) { - // Call TCP output & close the connection + http_send(&shared_buffer, pcb, 0); + } + else + { + TRACE("http.c: Unable to serve page - buffer at %d bytes", strlen(shared_buffer)); + } + + if(interfaceCreate_Display_Ports(2)) + { + http_send(&shared_buffer, pcb, 0); + } + else + { + TRACE("http.c: Unable to serve page - buffer at %d bytes", strlen(shared_buffer)); + } + + if(interfaceCreate_Display_Ports(3)) + { http_send(&shared_buffer, pcb, 1); TRACE("http.c: Page sent successfully - %d bytes", strlen(shared_buffer)); } @@ -722,10 +752,7 @@ static err_t http_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err eeprom_write(); TRACE("http.c: restarting the Zodiac FX. Please reconnect."); - for(int x = 0;x<100000;x++); // Let the above message get sent to the terminal before detaching - udc_detach(); // Detach the USB device before restart - rstc_start_software_reset(RSTC); // Software reset - while (1); + software_reset(); } else if(strcmp(post_msg,"save_ports") == 0) { @@ -819,7 +846,7 @@ static err_t http_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err { // Only write to buffer - don't send http_send(&shared_buffer, pcb, 0); - TRACE("http.c: updated ports page sent successfully (1/2) - %d bytes", strlen(shared_buffer)); + TRACE("http.c: updated ports page sent successfully (1/4) - %d bytes", strlen(shared_buffer)); } else { @@ -827,10 +854,32 @@ static err_t http_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err } if(interfaceCreate_Display_Ports(1)) + { + // Call TCP output & close the connection + http_send(&shared_buffer, pcb, 0); + TRACE("http.c: updated ports page sent successfully (2/4) - %d bytes", strlen(shared_buffer)); + } + else + { + TRACE("http.c: unable to serve updated page - buffer at %d bytes", strlen(shared_buffer)); + } + + if(interfaceCreate_Display_Ports(2)) + { + // Call TCP output & close the connection + http_send(&shared_buffer, pcb, 0); + TRACE("http.c: updated ports page sent successfully (3/4) - %d bytes", strlen(shared_buffer)); + } + else + { + TRACE("http.c: unable to serve updated page - buffer at %d bytes", strlen(shared_buffer)); + } + + if(interfaceCreate_Display_Ports(3)) { // Call TCP output & close the connection http_send(&shared_buffer, pcb, 1); - TRACE("http.c: updated ports page sent successfully (2/2) - %d bytes", strlen(shared_buffer)); + TRACE("http.c: updated ports page sent successfully (4/4) - %d bytes", strlen(shared_buffer)); } else { @@ -1156,20 +1205,22 @@ static err_t http_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err if(pdat[i+1] == 'w') { int oc1,oc2,oc3,oc4; - if (strlen(http_msg) > 15 ) + if (strlen(http_msg) <= 15 ) { - TRACE("http.c: incorrect IP format"); - return; - } - sscanf(http_msg, "%d.%d.%d.%d", &oc1,&oc2,&oc3,&oc4); - Zodiac_Config.OFIP_address[0] = oc1; - Zodiac_Config.OFIP_address[1] = oc2; - Zodiac_Config.OFIP_address[2] = oc3; - Zodiac_Config.OFIP_address[3] = oc4; - TRACE("http.c: openflow server address set to %d.%d.%d.%d" ,\ + sscanf(http_msg, "%d.%d.%d.%d", &oc1,&oc2,&oc3,&oc4); + Zodiac_Config.OFIP_address[0] = oc1; + Zodiac_Config.OFIP_address[1] = oc2; + Zodiac_Config.OFIP_address[2] = oc3; + Zodiac_Config.OFIP_address[3] = oc4; + TRACE("http.c: openflow server address set to %d.%d.%d.%d" ,\ Zodiac_Config.OFIP_address[0], Zodiac_Config.OFIP_address[1],\ Zodiac_Config.OFIP_address[2], Zodiac_Config.OFIP_address[3]\ - ); + ); + } + else + { + TRACE("http.c: incorrect IP format"); + } } else { @@ -1412,23 +1463,24 @@ void http_close(struct tcp_pcb *pcb) */ static uint8_t upload_handler(char *payload, int len) { - static char page[IFLASH_PAGE_SIZE] = {0}; // Storage for each page of data - static uint16_t saved_bytes = 0; // Persistent counter of unwritten data - uint16_t handled_bytes = 0; // Counter of handled data - static uint32_t total_handled_bytes = 0; // Counter of total handled data - static char boundary_ID[BOUNDARY_MAX_LEN] = {0}; // Storage for boundary ID + // Persistent local variables + static uint8_t pagebuff[PAGEBUFF_SIZE] = {0}; // Storage for each page of data + static uint8_t boundary_ID[BOUNDARY_MAX_LEN] = {0}; // Storage for boundary ID + static uint16_t pagebuff_index = 0; + + // Local variables + uint16_t payload_index = 0; + uint8_t boundary_ret = 0; if(payload == NULL || len == 0) { // Clean up upload handler (on interrupted/failed upload) - memset(&page, 0, IFLASH_PAGE_SIZE); // Clear page storage + memset(&pagebuff, 0, PAGEBUFF_SIZE); // Clear page storage memset(&boundary_ID, 0, BOUNDARY_MAX_LEN); // Clear boundary storage - saved_bytes = 0; // Clear saved byte counter file_upload = false; // Clear file upload flag boundary_start = 1; // Set starting boundary required flag upload_pcb = NULL; // Clear pcb connection pointer upload_timer = 0; // Clear upload timeout - total_handled_bytes = 0; return 1; } @@ -1542,7 +1594,7 @@ static uint8_t upload_handler(char *payload, int len) boundary_start = 0; // Clear page array before use - memset(&page, 0, IFLASH_PAGE_SIZE); // Clear shared_buffer + memset(&pagebuff, 0, PAGEBUFF_SIZE); // Clear shared_buffer } } else @@ -1551,277 +1603,62 @@ static uint8_t upload_handler(char *payload, int len) px = payload; } - // Search for ending boundary - py = payload + len; - - i = 128; - while(i>0) + // Fix alignment after boundary detection + if(px > payload) { - py--; - // Latch onto '----' ("----[boundary ID]") - if((*(py-1)) == '\x2d' && (*(py-2)) == '\x2d' && (*(py-3)) == '\x2d' && (*(py-4)) == '\x2d') - { - // Store the discovered boundary - char tmpID[BOUNDARY_MAX_LEN] = {0}; - int z = 0; - while(z < BOUNDARY_MAX_LEN && *(py+z) != '\x2d' && *(py+z) != '\x0d' && *(py+z) != '\x0a') - { - tmpID[z] = *(py+z); - z++; - } - - TRACE("http.c: discovered boundary ID : %s", tmpID); - - // Match the boundary ID with stored ID - if(strcmp(tmpID, boundary_ID) == 0) - { - TRACE("http.c: boundary IDs match"); - TRACE("http.c: moving data end pointer"); - // Traverse through the preceding newline characters - while(*(py-1) == '\x0d' || *(py-1) == '\x0a' || *(py-1) == '\x2d') - { - py--; - } - - i = 0; - // 'i' will be decremented to -1 if this line is run - } - else - { - TRACE("http.c: boundary IDs do not match"); - i = 1; - // 'i' will be decremented to 0 if this line is run - } - } - i--; + len = len - (px - payload); + payload = px; } - - if(i == 0) + while(payload_index < len) { - TRACE("http.c: ending boundary not found - ending data is valid"); - - // Return ending pointer to the end - py = payload + len; - } - else - { - TRACE("http.c: ending boundary found"); - final = 1; - } - - // Get length of uploaded part - data_len = py - px; - - // Check if any existing data needs to be handled - if(saved_bytes) - { - TRACE("http.c: %d saved bytes need to be cleared", saved_bytes); - - if(final) - { - /* Final page needs to be written */ - - // Fill 512-byte array - while(saved_bytes < IFLASH_PAGE_SIZE) - { - if(px < py) - { - // Write data - page[saved_bytes] = *px; - px++; - handled_bytes++; - } - else - { - // Append 0xFF - page[saved_bytes] = 0xFF; - } - - saved_bytes++; - } - - // Write data to page - if(flash_write_page(&page)) - { - TRACE("http.c: final firmware page written successfully"); - page_ctr++; - } - else - { - TRACE("http.c: final firmware page write FAILED"); - } - } - else if(saved_bytes + len < IFLASH_PAGE_SIZE) + if(pagebuff_index < PAGEBUFF_SIZE) { - int max_len = saved_bytes + len; - // Fill existing partially-complete page with new data - while(saved_bytes < max_len && handled_bytes < len) - { - page[saved_bytes] = *px; - if(px < py) - { - px++; - } - else - { - TRACE("http.c: ERROR - multi-part start pointer has passed the end pointer"); - } - saved_bytes++; - handled_bytes++; - } - - // Handle edge-case - TRACE("http.c: unable to fill a complete page - skipping page write"); - TRACE("http.c: %d bytes saved", saved_bytes); - - total_handled_bytes += handled_bytes; - return 1; + pagebuff[pagebuff_index] = payload[payload_index]; + pagebuff_index++; } - else + + if(pagebuff_index >= PAGEBUFF_SIZE) { - // Fill existing partially-complete page with new data - while(saved_bytes < IFLASH_PAGE_SIZE && handled_bytes < len) - { - page[saved_bytes] = *px; - if(px < py) - { - px++; - } - else - { - TRACE("http.c: ERROR - multi-part start pointer has passed the end pointer"); - } - saved_bytes++; - handled_bytes++; - } + boundary_ret = process_pagebuff(pagebuff, pagebuff_index, boundary_ID); + if(boundary_ret == 1) break; - // Write data to page - if(flash_write_page(&page)) - { - TRACE("http.c: firmware page written successfully"); - page_ctr++; - } - else - { - TRACE("http.c: firmware page write FAILED"); - } + pagebuff_index = send_pagebuff(pagebuff, pagebuff_index); } - - // Saved bytes have been handled - clear the counter - saved_bytes = 0; - TRACE("http.c: saved bytes have been cleared"); - TRACE("http.c: handled_bytes: %04d, data_len: %04d", handled_bytes, data_len); + payload_index++; } - - while(handled_bytes < data_len) + + if(boundary_ret == 0) { - if(data_len - handled_bytes >= IFLASH_PAGE_SIZE) - { - // Fill 512-byte array - int j = 0; - while(j < IFLASH_PAGE_SIZE) - { - page[j] = *px; - if(px < py) - { - px++; - } - else - { - TRACE("http.c: ERROR - multi-part start pointer has passed the end pointer"); - } - j++; - handled_bytes++; - } - - // Write to page - if(flash_write_page(&page)) - { - TRACE("http.c: firmware page written successfully"); - page_ctr++; - } - else - { - TRACE("http.c: firmware page write FAILED"); - } - } - else if(!final) + boundary_ret = process_pagebuff(pagebuff, pagebuff_index, boundary_ID); + } + + if(boundary_ret == 1) + { + if(pagebuff_index >= 512) { - /* Data needs to be saved */ - TRACE("http.c: data needs to be saved"); - - // Save leftover into page array for next run-through - int j = 0; - while(handled_bytes < data_len) + pagebuff_index = send_pagebuff(pagebuff, pagebuff_index); + for(pagebuff_index; pagebuff_index < IFLASH_PAGE_SIZE; pagebuff_index++) { - page[j] = *px; - if(px < py) - { - px++; - } - else - { - TRACE("http.c: ERROR - multi-part start pointer has passed the end pointer"); - } - j++; - handled_bytes++; - saved_bytes++; + pagebuff[pagebuff_index] = 0xFF; } - - TRACE("http.c: %d bytes saved", saved_bytes); + pagebuff_index = send_pagebuff(pagebuff, pagebuff_index); } else { - /* Final page needs to be written */ - - // Fill 512-byte array - int j = 0; - while(j < IFLASH_PAGE_SIZE) - { - if(px < py) - { - // Write data - page[j] = *px; - px++; - handled_bytes++; - } - else - { - // Append 0xFF - page[j] = 0xFF; - } - - j++; - } - - // Write to page - if(flash_write_page(&page)) - { - TRACE("http.c: final page written successfully"); - page_ctr++; - } - else - { - TRACE("http.c: final page write FAILED"); - } + pagebuff_index = send_pagebuff(pagebuff, pagebuff_index); } - - TRACE("http.c: handled_bytes: %04d, data_len: %04d", handled_bytes, data_len); - } - - total_handled_bytes += handled_bytes; - TRACE("http.c: total_handled_bytes: %d", total_handled_bytes); - - if(final) - { + return 2; } - else + + if(pagebuff_index >= 512) { - return 1; + pagebuff_index = send_pagebuff(pagebuff, pagebuff_index); } + + return 1; } static uint8_t Config_Network(char *payload, int len) @@ -1884,7 +1721,7 @@ static uint8_t Config_Network(char *payload, int len) if (strlen(http_msg) != 27 ) // Accounting for ":" as "%3A" { TRACE("http.c: incorrect MAC address format"); - return; + return FAILURE; } // Decode http string @@ -1942,7 +1779,7 @@ static uint8_t Config_Network(char *payload, int len) if (strlen(http_msg) > 15 ) { TRACE("http.c: incorrect IP format"); - return; + return FAILURE; } sscanf(http_msg, "%d.%d.%d.%d", &ip1, &ip2,&ip3,&ip4); Zodiac_Config.IP_address[0] = ip1; @@ -1981,7 +1818,7 @@ static uint8_t Config_Network(char *payload, int len) if (strlen(http_msg) > 15 ) { TRACE("http.c: incorrect netmask format"); - return; + return FAILURE; } sscanf(http_msg, "%d.%d.%d.%d", &nm1, &nm2,&nm3,&nm4); Zodiac_Config.netmask[0] = nm1; @@ -2021,7 +1858,7 @@ static uint8_t Config_Network(char *payload, int len) if (strlen(http_msg) > 15 ) { TRACE("http.c: incorrect gateway format"); - return; + return FAILURE; } sscanf(http_msg, "%d.%d.%d.%d", &gw1, &gw2,&gw3,&gw4); Zodiac_Config.gateway_address[0] = gw1; @@ -2080,11 +1917,9 @@ static uint8_t interfaceCreate_Frames(void) ""\ ""\ ); - TRACE("http.c: html written to buffer"); if(strlen(shared_buffer) < 2048) { - TRACE("http.c: http/html written to buffer"); return 1; } else @@ -2161,7 +1996,6 @@ static uint8_t interfaceCreate_Header(void) ""\ , hr, min) < SHARED_BUFFER_LEN) { - TRACE("http.c: html written to buffer"); return 1; } else @@ -2230,7 +2064,6 @@ static uint8_t interfaceCreate_Menu(void) ""\ ) < SHARED_BUFFER_LEN) { - TRACE("http.c: html written to buffer"); return 1; } else @@ -2249,6 +2082,8 @@ static uint8_t interfaceCreate_Home(void) int hr = (totaltime/2)/3600; int t = (totaltime/2)%3600; int min = t/60; + + sprintf(shared_buffer, http_header); @@ -2260,7 +2095,7 @@ static uint8_t interfaceCreate_Home(void) ""\ ""\ ""\ @@ -2269,17 +2104,20 @@ static uint8_t interfaceCreate_Home(void) "CPU UID: %d-%d-%d-%d
"\ "Firmware Version: %s
"\ "CPU Temp: %d C
"\ - "Uptime: %02d:%02d"\ + "Uptime: %02d:%02d
"\ + , uid_buf[0], uid_buf[1], uid_buf[2], uid_buf[3]\ + , VERSION, (int)ul_temp, hr, min\ + ); + + if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ "

"\ "
"\ ""\ "
"\ ""\ ""\ - , uid_buf[0], uid_buf[1], uid_buf[2], uid_buf[3]\ - , VERSION, (int)ul_temp, hr, min) < SHARED_BUFFER_LEN) + ) < SHARED_BUFFER_LEN ) { - TRACE("http.c: html written to buffer"); return 1; } else @@ -2321,7 +2159,6 @@ static uint8_t interfaceCreate_Upload(void) ""\ ) < SHARED_BUFFER_LEN) { - TRACE("http.c: html written to buffer"); return 1; } else @@ -2337,102 +2174,77 @@ static uint8_t interfaceCreate_Upload(void) */ static uint8_t interfaceCreate_Upload_Status(uint8_t sel) { + + snprintf(shared_buffer, SHARED_BUFFER_LEN,\ + ""\ + ""\ + ""\ + ""\ + ""\ + ""\ + "

"\ + "

Firmware Update

"\ + "

"\ + ""\ + ); if(sel == 1) - { - if( snprintf(shared_buffer, SHARED_BUFFER_LEN,\ - ""\ - ""\ - ""\ - ""\ - ""\ - ""\ - "

"\ - "

Firmware Update

"\ - "

"\ - ""\ - "

Firmware upload successful.

"\ - "Zodiac FX will be updated on the next restart.

"\ - "
"\ - ""\ - "
"\ - ""\ - ""\ - ) < SHARED_BUFFER_LEN) - { - TRACE("http.c: html written to buffer"); - return 1; - } - else - { - TRACE("http.c: WARNING: html truncated to prevent buffer overflow"); - return 0; - } + { + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + "

Firmware upload successful.

"\ + "Zodiac FX will be updated on the next restart.

"\ + "
"\ + ""\ + "
"\ + ); } - else + else if(sel == 2) { - snprintf(shared_buffer, SHARED_BUFFER_LEN,\ - ""\ - ""\ - ""\ - ""\ - ""\ - ""\ - "

"\ - "

Firmware Update

"\ - "

"\ - ""\ - ); - if(sel == 2) - { - snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ - "

Firmware upload interrupted. Please try again.

"\ - ); - } - else if(sel == 3) - { - snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ - "

Firmware upload failed. Unable to verify firmware. Please try again, or check the integrity of the firmware.

"\ - ); - } - else if(sel == 4) - { - snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ - "

Firmware upload in progress. Please try again in 30 seconds.

"\ + "

Firmware upload interrupted. Please try again.

"\ ); - } - else - { - snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ - "

Firmware upload failed. Please try again.

"\ + } + else if(sel == 3) + { + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + "

Firmware upload failed. Unable to verify firmware. Please try again, or check the integrity of the firmware.

"\ ); - } - - if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ - ""\ - ""\ - ) < SHARED_BUFFER_LEN) - { - TRACE("http.c: html written to buffer"); - return 1; - } - else - { - TRACE("http.c: WARNING: html truncated to prevent buffer overflow"); - return 0; - } } + else if(sel == 4) + { + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + "

Firmware upload in progress. Please try again in 30 seconds.

"\ + ); + } + else if(sel == 5) + { + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + "

Firmware upload failed. Zodiac FX supports binaries of up to 228k in size.

"\ + ); + } + else + { + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + "

Firmware upload failed. Please try again.

"\ + ); + } + + if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + ""\ + ""\ + ) < SHARED_BUFFER_LEN) + { + return 1; + } + else + { + TRACE("http.c: WARNING: html truncated to prevent buffer overflow"); + return 0; + } + } /* @@ -2477,7 +2289,6 @@ static uint8_t interfaceCreate_Display_Home(void) ""\ ) < SHARED_BUFFER_LEN) { - TRACE("http.c: html written to buffer"); return 1; } else @@ -2495,35 +2306,6 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) { if(step == 0) { - int currPort; - - // Create status strings - char portStatusch[2][5]; - snprintf(portStatusch[0], 5, "DOWN"); - snprintf(portStatusch[1], 5, "UP"); - - // Create VLAN type strings - char portvlType[3][11]; - snprintf(portvlType[0], 11, "n/a"); - snprintf(portvlType[1], 11, "OpenFlow"); - snprintf(portvlType[2], 11, "Native"); - - // Create - int vlArr[4] = { 0 }; - - // Count active VLANs, store list value in vlArr - int x; - uint8_t vlCtr = 0; - for (x=0;xPort 2"\ "Port 3"\ "Port 4"\ + ); + + if ( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ ""\ ""\ "Status:"\ + ) < SHARED_BUFFER_LEN) + { + return 1; + } + else + { + TRACE("http.c: WARNING: html truncated to prevent buffer overflow"); + return 0; + } + } + else if(step == 1) + { + int currPort; + + // Create status strings + char portStatusch[2][5]; + snprintf(portStatusch[0], 5, "DOWN"); + snprintf(portStatusch[1], 5, "UP"); + + // Create VLAN type strings + char portvlType[3][11]; + snprintf(portvlType[0], 11, "n/a"); + snprintf(portvlType[1], 11, "OpenFlow"); + snprintf(portvlType[2], 11, "Native"); + + // Create + int vlArr[4] = { 0 }; + + // Count active VLANs, store list value in vlArr + int x; + uint8_t vlCtr = 0; + for (x=0;x%s"\ "%s"\ "%s"\ "%s"\ + , portStatusch[port_status[0]], portStatusch[port_status[1]], portStatusch[port_status[2]], portStatusch[port_status[3]]\ + ); + + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ ""\ ""\ - "VLAN Type:"\ - , portStatusch[port_status[0]], portStatusch[port_status[1]], portStatusch[port_status[2]], portStatusch[port_status[3]]\ + "VLAN Type:"\ ); // Create VLAN type for each port @@ -2627,13 +2457,13 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) ); } } - + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ ""\ ""\ "VLAN ID:"\ ); - + // Create VLAN dropdown for each port for(x=0;x<4;x++) { @@ -2677,10 +2507,9 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) } if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ - ""\ + ""\ ) < SHARED_BUFFER_LEN) { - TRACE("http.c: html (1/2) written to buffer"); return 1; } else @@ -2689,77 +2518,124 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) return 0; } } - else if(step == 1) + else if(step == 2) { if(OF_Version == 1) { // of v1.0 - if( snprintf(shared_buffer, SHARED_BUFFER_LEN,\ + snprintf(shared_buffer, SHARED_BUFFER_LEN,\ ""\ "RX Bytes:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys10_port_stats[0].rx_bytes, phys10_port_stats[1].rx_bytes, phys10_port_stats[2].rx_bytes, phys10_port_stats[3].rx_bytes + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ "TX Bytes:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys10_port_stats[0].tx_bytes, phys10_port_stats[1].tx_bytes, phys10_port_stats[2].tx_bytes, phys10_port_stats[3].tx_bytes + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ "RX Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys10_port_stats[0].rx_packets, phys10_port_stats[1].rx_packets, phys10_port_stats[2].rx_packets, phys10_port_stats[3].rx_packets + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ "TX Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ + "%"PRIu64""\ + "%"PRIu64""\ + "%"PRIu64""\ + "%"PRIu64""\ + , phys10_port_stats[0].tx_packets, phys10_port_stats[1].tx_packets, phys10_port_stats[2].tx_packets, phys10_port_stats[3].tx_packets + ); + if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ + ""\ + ) < SHARED_BUFFER_LEN) + { + TRACE("http.c: html (2/2) written to buffer"); + return 1; + } + else + { + TRACE("http.c: WARNING: html truncated to prevent buffer overflow"); + return 0; + } + } + else + { + // of v1.3 + snprintf(shared_buffer, SHARED_BUFFER_LEN,\ + ""\ + "RX Bytes:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].rx_bytes, phys13_port_stats[1].rx_bytes, phys13_port_stats[2].rx_bytes, phys13_port_stats[3].rx_bytes + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ - "RX Dropped Packets:"\ + "TX Bytes:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].tx_bytes, phys13_port_stats[1].tx_bytes, phys13_port_stats[2].tx_bytes, phys13_port_stats[3].tx_bytes + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ - "TX Dropped Packets:"\ + "RX Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].rx_packets, phys13_port_stats[1].rx_packets, phys13_port_stats[2].rx_packets, phys13_port_stats[3].rx_packets + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ - "RX CRC Errors:"\ + "TX Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].tx_packets, phys13_port_stats[1].tx_packets, phys13_port_stats[2].tx_packets, phys13_port_stats[3].tx_packets + ); + if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ - ""\ - "
"\ - ""\ - "
"\ - ""\ - ""\ - ""\ - ""\ - , phys10_port_stats[0].rx_bytes, phys10_port_stats[1].rx_bytes, phys10_port_stats[2].rx_bytes, phys10_port_stats[3].rx_bytes - , phys10_port_stats[0].tx_bytes, phys10_port_stats[1].tx_bytes, phys10_port_stats[2].tx_bytes, phys10_port_stats[3].tx_bytes - , phys10_port_stats[0].rx_packets, phys10_port_stats[1].rx_packets, phys10_port_stats[2].rx_packets, phys10_port_stats[3].rx_packets - , phys10_port_stats[0].tx_packets, phys10_port_stats[1].tx_packets, phys10_port_stats[2].tx_packets, phys10_port_stats[3].tx_packets - , phys10_port_stats[0].rx_dropped, phys10_port_stats[1].rx_dropped, phys10_port_stats[2].rx_dropped, phys10_port_stats[3].rx_dropped - , phys10_port_stats[0].tx_dropped, phys10_port_stats[1].tx_dropped, phys10_port_stats[2].tx_dropped, phys10_port_stats[3].tx_dropped - , phys10_port_stats[0].rx_crc_err, phys10_port_stats[1].rx_crc_err, phys10_port_stats[2].rx_crc_err, phys10_port_stats[3].rx_crc_err - ) < SHARED_BUFFER_LEN) + ) < SHARED_BUFFER_LEN) { TRACE("http.c: html (2/2) written to buffer"); return 1; @@ -2770,58 +2646,107 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) return 0; } } - else + } + else if(step == 3) + { + if(OF_Version == 1) { - // of v1.3 - if( snprintf(shared_buffer, SHARED_BUFFER_LEN,\ - ""\ - "RX Bytes:"\ - "%"PRIu64""\ - "%"PRIu64""\ - "%"PRIu64""\ - "%"PRIu64""\ - ""\ + // of v1.0 + snprintf(shared_buffer, SHARED_BUFFER_LEN,\ ""\ - "TX Bytes:"\ + "RX Dropped Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys10_port_stats[0].rx_dropped, phys10_port_stats[1].rx_dropped, phys10_port_stats[2].rx_dropped, phys10_port_stats[3].rx_dropped + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ - "RX Packets:"\ + "TX Dropped Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys10_port_stats[0].tx_dropped, phys10_port_stats[1].tx_dropped, phys10_port_stats[2].tx_dropped, phys10_port_stats[3].tx_dropped + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ - "TX Packets:"\ + "RX CRC Errors:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys10_port_stats[0].rx_crc_err, phys10_port_stats[1].rx_crc_err, phys10_port_stats[2].rx_crc_err, phys10_port_stats[3].rx_crc_err + ); + if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ + ""\ + "
"\ + ""\ + "
"\ + ""\ + ""\ + ""\ + ""\ + ) < SHARED_BUFFER_LEN) + { + TRACE("http.c: html (2/2) written to buffer"); + return 1; + } + else + { + TRACE("http.c: WARNING: html truncated to prevent buffer overflow"); + return 0; + } + } + else + { + // of v1.3 + snprintf(shared_buffer, SHARED_BUFFER_LEN,\ ""\ "RX Dropped Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].rx_dropped, phys13_port_stats[1].rx_dropped, phys13_port_stats[2].rx_dropped, phys13_port_stats[3].rx_dropped + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ "TX Dropped Packets:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].tx_dropped, phys13_port_stats[1].tx_dropped, phys13_port_stats[2].tx_dropped, phys13_port_stats[3].tx_dropped + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ "RX CRC Errors:"\ + ); + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ "%"PRIu64""\ + , phys13_port_stats[0].rx_crc_err, phys13_port_stats[1].rx_crc_err, phys13_port_stats[2].rx_crc_err, phys13_port_stats[3].rx_crc_err + ); + if( snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN,\ ""\ ""\ "
"\ @@ -2831,13 +2756,6 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) ""\ ""\ ""\ - , phys13_port_stats[0].rx_bytes, phys13_port_stats[1].rx_bytes, phys13_port_stats[2].rx_bytes, phys13_port_stats[3].rx_bytes - , phys13_port_stats[0].tx_bytes, phys13_port_stats[1].tx_bytes, phys13_port_stats[2].tx_bytes, phys13_port_stats[3].tx_bytes - , phys13_port_stats[0].rx_packets, phys13_port_stats[1].rx_packets, phys13_port_stats[2].rx_packets, phys13_port_stats[3].rx_packets - , phys13_port_stats[0].tx_packets, phys13_port_stats[1].tx_packets, phys13_port_stats[2].tx_packets, phys13_port_stats[3].tx_packets - , phys13_port_stats[0].rx_dropped, phys13_port_stats[1].rx_dropped, phys13_port_stats[2].rx_dropped, phys13_port_stats[3].rx_dropped - , phys13_port_stats[0].tx_dropped, phys13_port_stats[1].tx_dropped, phys13_port_stats[2].tx_dropped, phys13_port_stats[3].tx_dropped - , phys13_port_stats[0].rx_crc_err, phys13_port_stats[1].rx_crc_err, phys13_port_stats[2].rx_crc_err, phys13_port_stats[3].rx_crc_err ) < SHARED_BUFFER_LEN) { TRACE("http.c: html (2/2) written to buffer"); @@ -2850,9 +2768,11 @@ static uint8_t interfaceCreate_Display_Ports(uint8_t step) } } } + else { - TRACE:("http.c: Display: Ports step error"); + TRACE("http.c: Display: Ports step error"); + return 0; } } @@ -2964,7 +2884,6 @@ static uint8_t interfaceCreate_Display_OpenFlow(void) , wi_ofStatus , wi_ofVersion , wi_ofTables , wi_ofFlows , wi_ofLookups , wi_ofMatches\ ) < SHARED_BUFFER_LEN) { - TRACE("http.c: html written to buffer"); return 1; } else @@ -2980,6 +2899,22 @@ static uint8_t interfaceCreate_Display_OpenFlow(void) */ static uint8_t interfaceCreate_Display_Flows(void) { + int i; + uint8_t flowEnd = flowBase + FLOW_DISPLAY_LIMIT; + struct ofp_action_header * act_hdr; + + // Ensure page correctly displays end of flows + if(iLastFlow < flowEnd) + { + flowEnd = iLastFlow; + } + + // Re-base start of flows if flows have been cleared + if(flowBase > iLastFlow) + { + flowBase = 0; + } + sprintf(shared_buffer, http_header); snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ @@ -2996,22 +2931,22 @@ static uint8_t interfaceCreate_Display_Flows(void) ""\ "

"\ "

Flows

"\ + "%d flows installed
"\ + , iLastFlow); + + if(iLastFlow != 0) + { + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ + "Showing flows %d - %d
"\ + , flowBase+1, flowEnd); + } + + snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),\ "

"\ "
"\
 			);
 
 // Begin Flow formatting
-
-int i;
-uint8_t flowEnd = flowBase + FLOW_DISPLAY_LIMIT;
-struct ofp_action_header * act_hdr;
-
-// Ensure page correctly displays end of flows
-if(iLastFlow < flowEnd)
-{
-	flowEnd = iLastFlow;
-}
-
 if (iLastFlow > 0)
 {
 	// OpenFlow v1.0 (0x01) Flow Table
@@ -3414,8 +3349,6 @@ if (iLastFlow > 0)
 		}
 		snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"_______\r\n\n");
 	}
-	} else {
-	snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"No Flows installed\r\n");
 	}
 	
 // End Flow formatting
@@ -3459,7 +3392,6 @@ if (iLastFlow > 0)
 		""\
 	) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: html written to buffer");
 		return 1;
 	}
 	else
@@ -3587,7 +3519,7 @@ static uint8_t interfaceCreate_Display_Meters(void)
 				}
 				else if(ptr_band->type == OFPMBT13_DSCP_REMARK)
 				{
-					snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"DSCP REMARK (unsupported)\r\n");
+					snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"DSCP REMARK\r\n");
 				}
 				else
 				{
@@ -3595,7 +3527,12 @@ static uint8_t interfaceCreate_Display_Meters(void)
 				}
 				snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"\t\t  Rate:\t\t%d\t\r\n", ptr_band->rate);
 				snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"\t\t  Burst Size:\t%d\t\r\n", ptr_band->burst_size);
-					
+				
+				if(ptr_band->type == OFPMBT13_DSCP_REMARK)
+				{
+					snprintf(shared_buffer+strlen(shared_buffer), SHARED_BUFFER_LEN-strlen(shared_buffer),"\t\t  Precedence:\t+%d\t\r\n", ((struct ofp13_meter_band_dscp_remark*)ptr_band)->prec_level);
+				}
+				
 				// Find band index
 				int band_index = ((uint8_t*)ptr_band - (uint8_t*)&(meter_entry[meter_index]->bands)) / sizeof(struct ofp13_meter_band_drop);
 					
@@ -3642,7 +3579,6 @@ static uint8_t interfaceCreate_Display_Meters(void)
 		""\
 	) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: html written to buffer");
 		return 1;
 	}
 	else
@@ -3690,7 +3626,6 @@ static uint8_t interfaceCreate_Config_Home(void)
 		""\
 		) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: html written to buffer");
 		return 1;
 	}
 	else
@@ -3748,7 +3683,6 @@ static uint8_t interfaceCreate_Config_Network(void)
 			, Zodiac_Config.gateway_address[0], Zodiac_Config.gateway_address[1], Zodiac_Config.gateway_address[2], Zodiac_Config.gateway_address[3]\
 		) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: html written to buffer");
 		return 1;
 	}
 	else
@@ -3866,7 +3800,6 @@ static uint8_t interfaceCreate_Config_VLANs(void)
 		""\
 		) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: VLAN base written to buffer");
 		return 1;
 	}
 	else
@@ -3999,7 +3932,6 @@ static uint8_t interfaceCreate_Config_OpenFlow(void)
 		""\
 		) < SHARED_BUFFER_LEN)
 		{
-			TRACE("http.c: OpenFlow Config page written to buffer");
 			return 1;
 		}
 		else
@@ -4042,7 +3974,6 @@ static uint8_t interfaceCreate_About(void)
 		""\
 				) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: html written to buffer");
 		return 1;
 	}
 	else
@@ -4078,7 +4009,6 @@ static uint8_t interfaceCreate_Restart(void)
 		""\
 				) < SHARED_BUFFER_LEN)
 	{
-		TRACE("http.c: html written to buffer");
 		return 1;
 	}
 	else
@@ -4086,4 +4016,91 @@ static uint8_t interfaceCreate_Restart(void)
 		TRACE("http.c: WARNING: html truncated to prevent buffer overflow");
 		return 0;
 	}
-}
\ No newline at end of file
+}
+
+static uint8_t	process_pagebuff(uint8_t * buff_addr, uint16_t buff_index, uint8_t * match_addr)
+{
+	if(buff_index < BOUNDARY_MAX_LEN)
+	{
+		return 0;
+	}
+	// Search for ending boundary
+	uint16_t tmp_index = buff_index-1;
+	while(tmp_index >= 4)
+	{
+		// Latch onto '----'
+		if(buff_addr[tmp_index-1] == '\x2d' && buff_addr[tmp_index-2] == '\x2d' && buff_addr[tmp_index-3] == '\x2d' && buff_addr[tmp_index-4] == '\x2d')
+		{
+			uint8_t tmp_ID[BOUNDARY_MAX_LEN] = {0};
+			
+			uint16_t i=0;
+			while(i 0 &&\
+							(buff_addr[tmp_index] == '\x0d'\
+							|| buff_addr[tmp_index] == '\x0a'\
+							|| buff_addr[tmp_index] == '\x2d'));
+				
+				tmp_index++;
+				while(tmp_index < PAGEBUFF_SIZE)
+				{
+					buff_addr[tmp_index] = 0xFF;
+					tmp_index++;
+				}
+				
+				return 1;
+			}
+			else
+			{
+				break;
+			}
+		}
+		
+		tmp_index--;
+	}
+	
+	
+	return 0;
+}
+
+static uint16_t	send_pagebuff(uint8_t * buff_addr, uint16_t buff_index)
+{
+	// Always writes 1 page
+	if(!flash_write_page(buff_addr))
+	{
+		TRACE("http.c: ERROR - page write failed");
+	}
+	
+	if(buff_index >= IFLASH_PAGE_SIZE)
+	{
+		// Copy unwritten bytes into bottom of array
+		uint16_t tmp_index = buff_index - IFLASH_PAGE_SIZE;
+		for(uint16_t i=0;ipayload, p->len, 128);
-	//gmac_write(p->payload, p->len, NativePortMatrix);
 }
 
 
diff --git a/ZodiacFX/src/main.c b/ZodiacFX/src/main.c
index 95494ae..4010674 100644
--- a/ZodiacFX/src/main.c
+++ b/ZodiacFX/src/main.c
@@ -51,8 +51,6 @@
 struct netif gs_net_if;
 struct zodiac_config Zodiac_Config;
 int charcount, charcount_last;
-bool masterselect;
-bool stackenabled;
 int portmap[4];
 int32_t ul_temp;
 uint8_t NativePortMatrix;
@@ -130,20 +128,6 @@ int main (void)
 	board_init();
 	get_serial(&uid_buf);
 
-	// Set up the GPIO pin for the Mater Select jumper
-	ioport_init();
-	ioport_set_pin_dir(MASTER_SEL, IOPORT_DIR_INPUT);
-
-	masterselect = ioport_get_pin_level(MASTER_SEL);	// true = slave
-	stacking_init(masterselect);	// Initialise the stacking connector as either master or slave
-
-	// Set the IRQ line as either master or slave
-	if(masterselect) {
-		ioport_set_pin_dir(SPI_IRQ1, IOPORT_DIR_OUTPUT);
-	} else {
-		ioport_set_pin_dir(SPI_IRQ1, IOPORT_DIR_INPUT);
-	}
-
 	irq_initialize_vectors(); // Initialize interrupt vector table support.
 
 	cpu_irq_enable(); // Enable interrupts
@@ -209,11 +193,7 @@ int main (void)
 	{
 		task_switch(&gs_net_if);
 		task_command(cCommand, cCommand_last);
-		// Only run the following tasks if set to Master
-		if(masterselect == false)
-		{
-			sys_check_timeouts();
-			task_openflow();	
-		} 
+		sys_check_timeouts();
+		task_openflow(); 
 	}
 }
diff --git a/ZodiacFX/src/openflow/of_helper.c b/ZodiacFX/src/openflow/of_helper.c
index db2f6b2..b7a9eef 100644
--- a/ZodiacFX/src/openflow/of_helper.c
+++ b/ZodiacFX/src/openflow/of_helper.c
@@ -52,8 +52,8 @@ extern int iLastFlow;
 extern int iLastMeter;
 extern int OF_Version;
 extern int totaltime;
-extern uint8_t last_port_status[4];
-extern uint8_t port_status[4];
+extern uint8_t last_port_status[TOTAL_PORTS];
+extern uint8_t port_status[TOTAL_PORTS];
 extern struct flows_counter flow_counters[MAX_FLOWS_13];
 extern struct table_counter table_counters[MAX_TABLES];
 extern struct meter_entry13 *meter_entry[MAX_METER_13];
@@ -140,7 +140,7 @@ void nnOF_timer(void)
 	} else if (timer_alt == 1){
 		update_port_status();
 		// If port status has changed send a port status message
-		for (int x=0;x<4;x++)
+		for (int x=0;xlast_packet_in = sys_get_ms();
 		
 		TRACE("of_helper.c: first hit of meter - packet not dropped");
-		return SUCCESS;
+		return METER_NOACT;
 	}
 	
 	// Find time delta
@@ -1335,7 +1331,7 @@ int	meter_handler(uint32_t id, uint16_t bytes)
 	else
 	{
 		TRACE("of_helper.c: unsupported meter configuration - packet not dropped");
-		return SUCCESS;
+		return METER_NOACT;
 	}
 	
 	// Check each band
@@ -1363,14 +1359,14 @@ int	meter_handler(uint32_t id, uint16_t bytes)
 	if(highest_rate == 0 || ptr_highest_band == NULL)
 	{
 		TRACE("of_helper.c: no bands triggered - packet not dropped");
-		return SUCCESS;
+		return METER_NOACT;
 	}
 	
 	// Check band type
-	if(ptr_highest_band->type != OFPMBT13_DROP)
+	if(ptr_highest_band->type != OFPMBT13_DROP && ptr_highest_band->type != OFPMBT13_DSCP_REMARK)
 	{
 		TRACE("of_helper.c: unsupported band type - not dropping packet");
-		return SUCCESS;
+		return METER_NOACT;
 	}
 	
 	TRACE("of_helper.c: highest triggered band rate:%d", highest_rate);
@@ -1383,8 +1379,22 @@ int	meter_handler(uint32_t id, uint16_t bytes)
 	band_stats_array[meter_index].band_stats[band_index].byte_band_count += bytes;
 	band_stats_array[meter_index].band_stats[band_index].packet_band_count++;
 
-	TRACE("of_helper.c: packet needs to be dropped");	
-	return FAILURE;
+	if(ptr_highest_band->type == OFPMBT13_DROP)
+	{
+		TRACE("of_helper.c: packet dropped");
+		return METER_DROP;
+	}
+	else if(ptr_highest_band->type == OFPMBT13_DSCP_REMARK)
+	{
+		struct ofp13_meter_band_dscp_remark * ptr_dscp_band = ptr_highest_band;
+		int prec_increase = (int)(ptr_dscp_band->prec_level);
+		
+		TRACE("of_helper.c: DSCP drop precedence needs to be increased by %d", prec_increase);
+		return prec_increase;
+	}
+	
+	TRACE("of_helper.c: ERROR - unknown band type");
+	return METER_NOACT;
 }
 
 /*
diff --git a/ZodiacFX/src/openflow/openflow.c b/ZodiacFX/src/openflow/openflow.c
index f34975b..27d4ffa 100644
--- a/ZodiacFX/src/openflow/openflow.c
+++ b/ZodiacFX/src/openflow/openflow.c
@@ -42,9 +42,9 @@
 
 // Global variables
 extern struct zodiac_config Zodiac_Config;
-extern uint8_t port_status[4];
-extern struct ofp10_port_stats phys10_port_stats[4];
-extern struct ofp13_port_stats phys13_port_stats[4];
+extern uint8_t port_status[TOTAL_PORTS];
+extern struct ofp10_port_stats phys10_port_stats[TOTAL_PORTS];
+extern struct ofp13_port_stats phys13_port_stats[TOTAL_PORTS];
 
 // Local Variables
 struct ofp_switch_config Switch_config;
diff --git a/ZodiacFX/src/openflow/openflow.h b/ZodiacFX/src/openflow/openflow.h
index ad8e60f..4c9eb18 100644
--- a/ZodiacFX/src/openflow/openflow.h
+++ b/ZodiacFX/src/openflow/openflow.h
@@ -123,11 +123,13 @@ void port_status_message13(uint8_t port);
 (((x) & 0xff000000UL) >> 24))
 #define NTOHL(x) HTONL(x)
 
-#define SHARED_BUFFER_LEN 2048
-
 #define	METER_PARTIAL	8		// Meter structure length, excluding header and bands
 
 #define SUCCESS		0
 #define FAILURE		1
 
+// Meter processing defines
+#define METER_DROP	-1	// drop packet
+#define METER_NOACT	0	// no action
+
 #endif /* OPENFLOW_H_ */
diff --git a/ZodiacFX/src/openflow/openflow_10.c b/ZodiacFX/src/openflow/openflow_10.c
index ec20e0c..c4f4310 100644
--- a/ZodiacFX/src/openflow/openflow_10.c
+++ b/ZodiacFX/src/openflow/openflow_10.c
@@ -52,8 +52,8 @@ extern struct table_counter table_counters[MAX_TABLES];
 extern int OF_Version;
 extern bool rcv_freq;
 extern uint8_t NativePortMatrix;
-extern struct ofp10_port_stats phys10_port_stats[4];
-extern uint8_t port_status[4];
+extern struct ofp10_port_stats phys10_port_stats[TOTAL_PORTS];
+extern uint8_t port_status[TOTAL_PORTS];
 extern uint8_t shared_buffer[SHARED_BUFFER_LEN];
 extern struct zodiac_config Zodiac_Config;
 extern struct ofp_switch_config Switch_config;
@@ -101,7 +101,7 @@ void nnOF10_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port)
 	uint16_t packet_size;
 	struct packet_fields fields = {0};
 	packet_fields_parser(p_uc_data, &fields);
-		
+
 	memcpy(&packet_size, ul_size, 2);
 	uint16_t eth_prot;
 	memcpy(ð_prot, p_uc_data + 12, 2);
@@ -444,13 +444,12 @@ void features_reply10(uint32_t xid)
 	features.n_tables = 1;		// Number of flow tables
 	features.capabilities = htonl(OFPC10_FLOW_STATS + OFPC10_TABLE_STATS + OFPC10_PORT_STATS);	// Switch Capabilities
 	features.actions = htonl((1 << OFPAT10_OUTPUT) + (1 << OFPAT10_SET_VLAN_VID) + (1 << OFPAT10_SET_DL_SRC) + (1 << OFPAT10_SET_DL_DST) + (1 << OFPAT10_SET_NW_SRC) + (1 << OFPAT10_SET_NW_DST) + (1 << OFPAT10_SET_TP_SRC) + (1 << OFPAT10_SET_TP_DST));		// Action Capabilities
-
 	uint8_t mac[] = {0x00,0x00,0x00,0x00,0x00,0x00};
 
 	memcpy(&buf, &features, sizeof(struct ofp10_switch_features));
 	update_port_status();		//update port status
 
-	for(l = 0; l< 4; l++)
+	for(l = 0; lbody;
 	int stats_size = 0;
-	int k, len;
-	char buf[1024];
+	int len = 0;
 	int port = ntohs(port_req->port_no);
+	uint8_t * buffer = shared_buffer;	// Local position index
+
+	// Clear shared_buffer
+	memset(&shared_buffer, 0, SHARED_BUFFER_LEN);
 
 	if (port == OFPP_NONE)
 	{
-		stats_size = (sizeof(struct ofp10_port_stats) * 3);
-		len = sizeof(struct ofp10_stats_reply) + stats_size;
+		// Find number of OpenFlow ports present
+		uint8_t ofports = 0;
+		for(uint8_t k=0; k 0 && port <= TOTAL_PORTS)	// Respond to request for ports
 	{
-		stats_size = sizeof(struct ofp10_port_stats);
-		len = sizeof(struct ofp10_stats_reply) + stats_size;
-
-		reply.header.version = OF_Version;
-		reply.header.type = OFPT10_STATS_REPLY;
-		reply.header.length = htons(len);
-		reply.header.xid = msg->header.xid;
-		reply.type = htons(OFPST_PORT);
-		reply.flags = 0;
-
-		zodiac_port_stats[port].port_no = htons(port);
-		zodiac_port_stats[port].rx_packets = htonll(phys10_port_stats[port-1].rx_packets);
-		zodiac_port_stats[port].tx_packets = htonll(phys10_port_stats[port-1].tx_packets);
-		zodiac_port_stats[port].rx_bytes = htonll(phys10_port_stats[port-1].rx_bytes);
-		zodiac_port_stats[port].tx_bytes = htonll(phys10_port_stats[port-1].tx_bytes);
-		zodiac_port_stats[port].rx_crc_err = htonll(phys10_port_stats[port-1].rx_crc_err);
-		zodiac_port_stats[port].rx_dropped = htonll(phys10_port_stats[port-1].rx_dropped);
-		zodiac_port_stats[port].tx_dropped = htonll(phys10_port_stats[port-1].tx_dropped);
-		zodiac_port_stats[port].rx_frame_err = 0;
-		zodiac_port_stats[port].rx_over_err = 0;
-		zodiac_port_stats[port].tx_errors = 0;
-		zodiac_port_stats[port].rx_errors = 0;
-		zodiac_port_stats[port].collisions = 0;
-
-		memcpy(buf, &reply, sizeof(struct ofp10_stats_reply));
-		memcpy(buf + sizeof(struct ofp10_stats_reply), &zodiac_port_stats[port], stats_size);
+		// Check if port is NOT native
+		if(!(NativePortMatrix & (1<<(port-1))))
+		{
+			stats_size = sizeof(struct ofp10_port_stats);
+			len = sizeof(struct ofp10_stats_reply) + stats_size;
+
+			reply.header.version = OF_Version;
+			reply.header.type = OFPT10_STATS_REPLY;
+			reply.header.length = htons(len);
+			reply.header.xid = msg->header.xid;
+			reply.type = htons(OFPST_PORT);
+			reply.flags = 0;
+
+			zodiac_port_stats.port_no = htons(port);
+			zodiac_port_stats.rx_packets = htonll(phys10_port_stats[port-1].rx_packets);
+			zodiac_port_stats.tx_packets = htonll(phys10_port_stats[port-1].tx_packets);
+			zodiac_port_stats.rx_bytes = htonll(phys10_port_stats[port-1].rx_bytes);
+			zodiac_port_stats.tx_bytes = htonll(phys10_port_stats[port-1].tx_bytes);
+			zodiac_port_stats.rx_crc_err = htonll(phys10_port_stats[port-1].rx_crc_err);
+			zodiac_port_stats.rx_dropped = htonll(phys10_port_stats[port-1].rx_dropped);
+			zodiac_port_stats.tx_dropped = htonll(phys10_port_stats[port-1].tx_dropped);
+			zodiac_port_stats.rx_frame_err = 0;
+			zodiac_port_stats.rx_over_err = 0;
+			zodiac_port_stats.tx_errors = 0;
+			zodiac_port_stats.rx_errors = 0;
+			zodiac_port_stats.collisions = 0;
+
+			memcpy(shared_buffer, &reply, sizeof(struct ofp10_stats_reply));
+			memcpy(shared_buffer + sizeof(struct ofp10_stats_reply), &zodiac_port_stats, stats_size);
+		}
+		else
+		{
+			TRACE("openflow_10.c: requested port is out of range");
+			of10_error(buffer, OFPET10_BAD_REQUEST, OFPBRC10_BAD_STAT);
+		}
 	}
-	sendtcp(&buf, len);
+	else
+	{
+		TRACE("openflow_10.c: requested port is out of range");
+		of10_error(buffer, OFPET10_BAD_REQUEST, OFPBRC10_BAD_STAT);
+	}
+	sendtcp(&shared_buffer, len);
 	return;
 }
 
@@ -788,6 +833,7 @@ void packet_in(uint8_t *buffer, uint16_t ul_size, uint8_t port, uint8_t reason)
 */
 void flow_mod(struct ofp_header *msg)
 {
+	///**/TRACE("____________________ FLOWMOD ENTRY");
 	struct ofp_flow_mod * ptr_fm;
 	ptr_fm = (struct ofp_flow_mod *) msg;
 
@@ -796,6 +842,7 @@ void flow_mod(struct ofp_header *msg)
 	{
 
 		case OFPFC_ADD:
+		///**/TRACE("____________________ ADD");
 		flow_add(msg);
 		break;
 
@@ -852,7 +899,7 @@ void flow_add(struct ofp_header *msg)
 		of10_error(msg, OFPET10_FLOW_MOD_FAILED, OFPFMFC10_ALL_TABLES_FULL);
 		return;
 	}
-	TRACE("Allocating %d bytes at %p for flow %d\r\n", sizeof(struct ofp_flow_mod), iLastFlow+1);
+	TRACE("Allocating %d bytes at %p for flow %d", sizeof(struct ofp_flow_mod), flow_match10[iLastFlow], iLastFlow+1);
 	
 	memcpy(flow_match10[iLastFlow], ptr_fm, sizeof(struct ofp_flow_mod));
 
diff --git a/ZodiacFX/src/openflow/openflow_13.c b/ZodiacFX/src/openflow/openflow_13.c
index 7de7391..c6ed7f9 100644
--- a/ZodiacFX/src/openflow/openflow_13.c
+++ b/ZodiacFX/src/openflow/openflow_13.c
@@ -58,9 +58,9 @@ extern uint8_t *ofp13_oxm_match[MAX_FLOWS_13];
 extern uint8_t *ofp13_oxm_inst[MAX_FLOWS_13];
 extern uint16_t ofp13_oxm_inst_size[MAX_FLOWS_13];
 extern struct flows_counter flow_counters[MAX_FLOWS_13];
-extern struct ofp13_port_stats phys13_port_stats[4];
+extern struct ofp13_port_stats phys13_port_stats[TOTAL_PORTS];
 extern struct table_counter table_counters[MAX_TABLES];
-extern uint8_t port_status[4];
+extern uint8_t port_status[TOTAL_PORTS];
 extern struct ofp_switch_config Switch_config;
 extern uint8_t shared_buffer[SHARED_BUFFER_LEN];
 extern int multi_pos;
@@ -146,11 +146,63 @@ void nnOF13_tablelookup(uint8_t *p_uc_data, uint32_t *ul_size, int port)
 		if(insts[OFPIT13_METER] != NULL)
 		{
 			struct ofp13_instruction_meter *inst_meter = insts[OFPIT13_METER];
-			if(meter_handler(ntohl(inst_meter->meter_id), packet_size) == FAILURE)	// Process meter id (provide byte count for counters)
+			int meter_ret = meter_handler(ntohl(inst_meter->meter_id), packet_size);
+			if(meter_ret == METER_DROP)	// Process meter id (provide byte count for counters)
 			{
 				// Packet must be dropped
+				TRACE("openflow_13.c: dropping packet");
 				return;
 			}
+			else if(meter_ret == METER_NOACT)
+			{
+				TRACE("openflow_13.c: no action taken");
+			}
+			else
+			{
+				if(meter_ret > 0)
+				{
+					int prec_increase = meter_ret;
+					
+					if (fields.eth_prot == htons(0x0800))
+					{
+						TRACE("openflow_13.c: increasing encoded drop precedence by %d", prec_increase);
+
+						// Retrieve TOS field
+						struct ip_hdr *hdr = fields.payload;
+						uint8_t prec_level = IPH_TOS(hdr);
+						TRACE("openflow_13.c: header current TOS field - %d", (int)prec_level);
+						// Isolate the drop precedence value (3 bits)
+						prec_level = (prec_level & 0x1C) >> 2;
+						// Check that value is valid ( 2 || 4 || 6 )
+						if( prec_level == 2 || prec_level == 4 || prec_level == 6)
+						{
+							// Increase drop precedence level by specified value
+							TRACE("openflow_13.c: increasing drop precedence level by %d", prec_increase);
+							prec_level = 2*(prec_level/2 + prec_increase);
+							// Ensure drop precedence value is valid
+							if(prec_level > 6)
+							{
+								prec_level = 6;
+							}
+							// Write new precedence to TOS field
+							TRACE("openflow_13.c: header new TOS field - %d", (prec_level<<2)|(IPH_TOS(hdr)&0xE3))
+							IPH_TOS_SET(hdr, (prec_level<<2)|(IPH_TOS(hdr)&0xE3));
+													
+							// Recalculate IP checksum
+							set_ip_checksum(p_uc_data, packet_size, fields.payload + 14);
+						}
+						else
+						{
+							TRACE("openflow_13.c: invalid drop precedence value - no adjustments made");
+						}
+
+					}
+				}
+				else
+				{
+					TRACE("openflow_13.c: ERROR - unhandled meter_handler return value");
+				}
+			}
 		}
 			
 		if(insts[OFPIT13_APPLY_ACTIONS] != NULL)
@@ -617,11 +669,6 @@ void of13_message(struct ofp_header *ofph, int size, int len)
 void features_reply13(uint32_t xid)
 {
 	uint64_t datapathid = 0;
-	int numofports = 0;
-	for(int n=0;n<4;n++)
-	{
-		if(Zodiac_Config.of_port[n]==1)numofports++;
-	}
 	struct ofp13_switch_features features;
 	uint8_t buf[256];
 	int bufsize = sizeof(struct ofp13_switch_features);
@@ -763,6 +810,13 @@ int multi_flow_reply13(uint8_t *buffer, struct ofp13_multipart_request *msg)
 */
 void multi_flow_more_reply13(void)
 {
+	uint16_t sndbuf = tcp_sndbuf(tcp_pcb);
+	if(sndbuf < 2048)
+	{
+		TRACE("openflow_13.c: waiting to reply with more flows, sndbuf @ %d", sndbuf);
+		return;
+	}
+	
 	// Clear shared_buffer
 	memset(&shared_buffer, 0, SHARED_BUFFER_LEN);
 	
@@ -851,7 +905,7 @@ int multi_aggregate_reply13(uint8_t *buffer, struct ofp13_multipart_request *msg
 int multi_portdesc_reply13(uint8_t *buffer, struct ofp13_multipart_request *msg)
 {
 	int numofports = 0;
-	for(int n=0;n<4;n++)
+	for(int n=0;nbody;
 	int stats_size = 0;
 	int len = 0;
@@ -1068,62 +1122,110 @@ int multi_portstats_reply13(uint8_t *buffer, struct ofp13_multipart_request *msg
 
 	if (port == OFPP13_ANY)
 	{
-		stats_size = (sizeof(struct ofp13_port_stats) * 3);	// Assumes 3 ports
-		len = sizeof(struct ofp13_multipart_reply) + stats_size;
-
-		reply.header.version = OF_Version;
-		reply.header.type = OFPT13_MULTIPART_REPLY;
-		reply.header.length = htons(len);
-		reply.header.xid = msg->header.xid;
-		reply.type = htons(OFPMP13_PORT_STATS);
-		reply.flags = 0;
-
-		for(int k=0; k<3;k++)
+		// Find number of OpenFlow ports present
+		uint8_t ofports = 0;
+		for(uint8_t k=0; kheader.xid;
 		reply.type = htons(OFPMP13_PORT_STATS);
 		reply.flags = 0;
-
-		zodiac_port_stats[port].port_no = htonl(port);
-		zodiac_port_stats[port].rx_packets = htonll(phys13_port_stats[port-1].rx_packets);
-		zodiac_port_stats[port].tx_packets = htonll(phys13_port_stats[port-1].tx_packets);
-		zodiac_port_stats[port].rx_bytes = htonll(phys13_port_stats[port-1].rx_bytes);
-		zodiac_port_stats[port].tx_bytes = htonll(phys13_port_stats[port-1].tx_bytes);
-		zodiac_port_stats[port].rx_crc_err = htonll(phys13_port_stats[port-1].rx_crc_err);
-		zodiac_port_stats[port].rx_dropped = htonll(phys13_port_stats[port-1].rx_dropped);
-		zodiac_port_stats[port].tx_dropped = htonll(phys13_port_stats[port-1].tx_dropped);
-		zodiac_port_stats[port].rx_frame_err = 0;
-		zodiac_port_stats[port].rx_over_err = 0;
-		zodiac_port_stats[port].tx_errors = 0;
-		zodiac_port_stats[port].rx_errors = 0;
-		zodiac_port_stats[port].collisions = 0;
-
+		
+		// Write reply header to buffer
 		memcpy(buffer, &reply, sizeof(struct ofp13_multipart_reply));
-		memcpy(buffer+sizeof(struct ofp13_multipart_reply), &zodiac_port_stats[port], stats_size);
+		// Increment buffer pointer
+		buffer += sizeof(struct ofp13_multipart_reply);
+		
+		// Write port stats to reply message
+		for(uint8_t k=0; k 0 && port <= TOTAL_PORTS)	// Respond to request for ports
+	{
+		// Check if port is NOT native
+		if(!(NativePortMatrix & (1<<(port-1))))
+		{
+			stats_size = sizeof(struct ofp13_port_stats);
+			len = sizeof(struct ofp13_multipart_reply) + stats_size;
+
+			reply.header.version = OF_Version;
+			reply.header.type = OFPT13_MULTIPART_REPLY;
+			reply.header.length = htons(len);
+			reply.header.xid = msg->header.xid;
+			reply.type = htons(OFPMP13_PORT_STATS);
+			reply.flags = 0;
+
+			zodiac_port_stats.port_no = htonl(port);
+			zodiac_port_stats.rx_packets = htonll(phys13_port_stats[port-1].rx_packets);
+			zodiac_port_stats.tx_packets = htonll(phys13_port_stats[port-1].tx_packets);
+			zodiac_port_stats.rx_bytes = htonll(phys13_port_stats[port-1].rx_bytes);
+			zodiac_port_stats.tx_bytes = htonll(phys13_port_stats[port-1].tx_bytes);
+			zodiac_port_stats.rx_crc_err = htonll(phys13_port_stats[port-1].rx_crc_err);
+			zodiac_port_stats.rx_dropped = htonll(phys13_port_stats[port-1].rx_dropped);
+			zodiac_port_stats.tx_dropped = htonll(phys13_port_stats[port-1].tx_dropped);
+			zodiac_port_stats.rx_frame_err = 0;
+			zodiac_port_stats.rx_over_err = 0;
+			zodiac_port_stats.tx_errors = 0;
+			zodiac_port_stats.rx_errors = 0;
+			zodiac_port_stats.collisions = 0;
+
+			memcpy(buffer, &reply, sizeof(struct ofp13_multipart_reply));
+			memcpy(buffer+sizeof(struct ofp13_multipart_reply), &zodiac_port_stats, stats_size);
+		}
+		else
+		{
+			TRACE("openflow_13.c: requested port is out of range");
+			of_error13(buffer, OFPET13_BAD_REQUEST, OFPBRC13_BAD_PORT);
+		}
+	}
+	else
+	{
+		TRACE("openflow_13.c: requested port is out of range");
+		of_error13(buffer, OFPET13_BAD_REQUEST, OFPBRC13_BAD_PORT);
 	}
 	return len;
 }
@@ -1487,7 +1589,7 @@ int multi_meter_features_reply13(uint8_t *buffer, struct ofp13_multipart_request
 	
 	// Format reply with meter features
 	meter_features.max_meter	= htonl(MAX_METER_13);
-	meter_features.band_types	= htonl(2);		// Only OFPMBT_DROP supported
+	meter_features.band_types	= htonl(1<rate			= ntohl(ptr_rxband->rate);
 			ptr_band->burst_size	= ntohl(ptr_rxband->burst_size);
 			
+			// Copy DSCP precedence level
+			if(ptr_band->type == OFPMBT13_DSCP_REMARK)
+			{
+				((struct ofp13_meter_band_dscp_remark*)ptr_band)->prec_level = ((struct ofp13_meter_band_dscp_remark*)ptr_rxband)->prec_level;
+			}
+			
 			ptr_band++;		// Move to next band storage location
 			ptr_rxband++;	// Move to next received band
 			bands_processed++;
@@ -2210,6 +2318,12 @@ void meter_modify13(struct ofp_header *msg)
 			ptr_band->rate			= ntohl(ptr_rxband->rate);
 			ptr_band->burst_size	= ntohl(ptr_rxband->burst_size);
 			
+			// Copy DSCP precedence level
+			if(ptr_band->type == OFPMBT13_DSCP_REMARK)
+			{
+				((struct ofp13_meter_band_dscp_remark*)ptr_band)->prec_level = ((struct ofp13_meter_band_dscp_remark*)ptr_rxband)->prec_level;
+			}
+			
 			// ***** TODO : add error checking for band processing
 			TRACE("openflow_13.c: %d of %d bands processed", bands_processed, bands_received);
 			
diff --git a/ZodiacFX/src/openflow_spec/openflow_spec10.h b/ZodiacFX/src/openflow_spec/openflow_spec10.h
index eed501a..0443d05 100644
--- a/ZodiacFX/src/openflow_spec/openflow_spec10.h
+++ b/ZodiacFX/src/openflow_spec/openflow_spec10.h
@@ -827,81 +827,4 @@ struct ofp_vendor_header {
     /* Vendor-defined arbitrary additional data. */
 };
 
-/* All ones is used to indicate all queues in a port (for stats retrieval). */
-#define OFPQ_ALL      0xffffffff
-
-/* Min rate > 1000 means not configured. */
-#define OFPQ_MIN_RATE_UNCFG      0xffff
-
-enum ofp_queue_properties {
-    OFPQT_NONE = 0,       /* No property defined for queue (default). */
-    OFPQT_MIN_RATE,       /* Minimum datarate guaranteed. */
-                          /* Other types should be added here
-                           * (i.e. max rate, precedence, etc). */
-};
-
-/* Common description for a queue. */
-struct ofp_queue_prop_header {
-    uint16_t property;    /* One of OFPQT_. */
-    uint16_t len;         /* Length of property, including this header. */
-    uint8_t pad[4];       /* 64-bit alignemnt. */
-};
-
-/* Min-Rate queue property description. */
-struct ofp_queue_prop_min_rate {
-    struct ofp_queue_prop_header prop_header; /* prop: OFPQT_MIN, len: 16. */
-    uint16_t rate;        /* In 1/10 of a percent; >1000 -> disabled. */
-    uint8_t pad[6];       /* 64-bit alignment */
-};
-
-/* Full description for a queue. */
-struct ofp_packet_queue {
-    uint32_t queue_id;     /* id for the specific queue. */
-    uint16_t len;          /* Length in bytes of this queue desc. */
-    uint8_t pad[2];        /* 64-bit alignment. */
-    struct ofp_queue_prop_header properties[0]; /* List of properties. */
-};
-
-/* Query for port queue configuration. */
-struct ofp_queue_get_config_request {
-    struct ofp_header header;
-    uint16_t port;         /* Port to be queried. Should refer
-                              to a valid physical port (i.e. < OFPP_MAX) */
-    uint8_t pad[2];        /* 32-bit alignment. */
-};
-
-/* Queue configuration for a given port. */
-struct ofp_queue_get_config_reply {
-    struct ofp_header header;
-    uint16_t port;
-    uint8_t pad[6];
-    struct ofp_packet_queue queues[0]; /* List of configured queues. */
-};
-
-/* OFPAT_ENQUEUE action struct: send packets to given queue on port. */
-struct ofp_action_enqueue {
-    uint16_t type;            /* OFPAT_ENQUEUE. */
-    uint16_t len;             /* Len is 16. */
-    uint16_t port;            /* Port that queue belongs. Should
-                                 refer to a valid physical port
-                                 (i.e. < OFPP_MAX) or OFPP_IN_PORT. */
-    uint8_t pad[6];           /* Pad for 64-bit alignment. */
-    uint32_t queue_id;        /* Where to enqueue the packets. */
-};
-
-struct ofp_queue_stats_request {
-    uint16_t port_no;        /* All ports if OFPT_ALL. */
-    uint8_t pad[2];          /* Align to 32-bits. */
-    uint32_t queue_id;       /* All queues if OFPQ_ALL. */
-};
-
-struct ofp_queue_stats {
-    uint16_t port_no;
-    uint8_t pad[2];          /* Align to 32-bits. */
-    uint32_t queue_id;       /* Queue i.d */
-    uint64_t tx_bytes;       /* Number of transmitted bytes. */
-    uint64_t tx_packets;     /* Number of transmitted packets. */
-    uint64_t tx_errors;      /* Number of packets dropped due to overrun. */
-};
-
 #endif /* OPENFLOW_10_H_ */
diff --git a/ZodiacFX/src/openflow_spec/openflow_spec13.h b/ZodiacFX/src/openflow_spec/openflow_spec13.h
index 7905b94..df6d5e1 100644
--- a/ZodiacFX/src/openflow_spec/openflow_spec13.h
+++ b/ZodiacFX/src/openflow_spec/openflow_spec13.h
@@ -840,7 +840,7 @@ struct ofp13_meter_band_drop {
 };
 
 /* OFPMBT_DSCP_REMARK band - Remark DSCP in the IP header */
-struct ofp_meter_band_dscp_remark {
+struct ofp13_meter_band_dscp_remark {
     uint16_t        type;    /* OFPMBT_DSCP_REMARK. */
     uint16_t        len;     /* Length in bytes of this band. */
     uint32_t        rate;    /* Rate for remarking packets. */
diff --git a/ZodiacFX/src/switch.c b/ZodiacFX/src/switch.c
index 00f9c42..533262d 100644
--- a/ZodiacFX/src/switch.c
+++ b/ZodiacFX/src/switch.c
@@ -35,29 +35,29 @@
 #include "switch.h"
 #include "conf_eth.h"
 #include "command.h"
+#include "timers.h"
 
 #include "ksz8795clx/ethernet_phy.h"
 #include "netif/etharp.h"
 
-/** The GMAC driver instance */
-gmac_device_t gs_gmac_dev;
+// Global variables
 extern struct tcp_conn tcp_conn;
 extern struct zodiac_config Zodiac_Config;
 extern int OF_Version;
-uint8_t gmacbuffer[GMAC_FRAME_LENTGH_MAX];
-uint8_t spibuffer[1];
-struct ofp10_port_stats phys10_port_stats[4];
-struct ofp13_port_stats phys13_port_stats[4];
-uint8_t port_status[4];
-uint8_t last_port_status[4];
 extern uint8_t NativePortMatrix;
-extern bool masterselect;
-extern bool stackenabled;
-/** Buffer for ethernet packets */
-static volatile uint8_t gs_uc_eth_buffer[GMAC_FRAME_LENTGH_MAX];
+extern uint8_t pending_spi_command;
+extern struct spi_packet *spi_packet;
+extern uint8_t shared_buffer[SHARED_BUFFER_LEN];
 
-/* SPI clock setting (Hz). */
-static uint32_t gs_ul_spi_clock = 500000;
+// Local variables
+gmac_device_t gs_gmac_dev;
+uint8_t gmacbuffer[GMAC_FRAME_LENTGH_MAX];
+struct ofp10_port_stats phys10_port_stats[TOTAL_PORTS];
+struct ofp13_port_stats phys13_port_stats[TOTAL_PORTS];
+uint8_t port_status[TOTAL_PORTS];
+uint8_t last_port_status[TOTAL_PORTS];
+static volatile uint8_t gs_uc_eth_buffer[GMAC_FRAME_LENTGH_MAX];
+uint8_t stats_rr = 0;
 
 /* GMAC HW configurations */
 #define BOARD_GMAC_PHY_ADDR 0
@@ -66,45 +66,6 @@ static uint32_t gs_ul_spi_clock = 500000;
 #define USART_SPI_DEVICE_ID         1
 #define USART_SPI_BAUDRATE          1000000
 
-/* Chip select. */
-#define SPI_CHIP_SEL 0
-#define SPI_CHIP_PCS spi_get_pcs(SPI_CHIP_SEL)
-/* Clock polarity. */
-#define SPI_CLK_POLARITY 0
-/* Clock phase. */
-#define SPI_CLK_PHASE 0
-/* Delay before SPCK. */
-#define SPI_DLYBS 0x40
-/* Delay between consecutive transfers. */
-#define SPI_DLYBCT 0x10
-
-#define SPI_SLAVE_PREAMBLE		0xAAAAAAAB
-#define SPI_MASTER_PREAMBLE		0xBBBBBBBC
-#define SPI_STATE_PREAMBLE	0
-#define SPI_STATE_COMMAND	1
-#define SPI_STATE_DATA		3
-
-uint8_t stats_rr = 0;
-
-uint32_t *spi_cmd_buffer;
-uint32_t spi_slv_preamble;
-uint8_t spi_state = 0;
-uint16_t spi_data_count = 0;
-uint16_t spi_command, spi_command_size;
-bool spi_slave_send; 
-uint16_t spi_slave_send_size;
-
-// Internal functions
-int readtxbytes(int port);
-int readrxbytes(int port);
-int readtxdrop(int port);
-int readrxdrop(int port);
-int readrxcrcerr(int port);
-void spi_master_initialize(void);
-void spi_slave_initialize(void);
-void stack_mst_write(uint8_t *rx_data, uint16_t ul_size);
-
-
 struct usart_spi_device USART_SPI_DEVICE = {
 	 /* Board specific select ID. */
 	 .id = USART_SPI_DEVICE_ID
@@ -122,308 +83,6 @@ void spi_init(void)
 	usart_spi_enable(USART_SPI);
 }
 
-
-void MasterStackSend(uint8_t *p_uc_data, uint16_t ul_size)
-{
-	uint32_t cmd_buffer;
-	
-	// Send the preamble mark the beginning of a transfer
-	cmd_buffer = ntohl(SPI_SLAVE_PREAMBLE);
-	stack_mst_write(&cmd_buffer, sizeof(cmd_buffer));
-	
-	// Send a 2 byte command code and 2 byte data code
-	cmd_buffer = ntohl(ul_size);
-	stack_mst_write(&cmd_buffer, sizeof(cmd_buffer));
-	
-	// Send packet
-	stack_mst_write(p_uc_data, ul_size);
-	return;
-}
-
-void MasterStackRcv(void)
-{
-	uint32_t cmd_buffer;
-	
-	TRACE("switch.c: Master received slave IRQ!");
-	// Send the preamble mark the beginning of a transfer
-	cmd_buffer = ntohl(SPI_MASTER_PREAMBLE);
-	stack_mst_write(&cmd_buffer, sizeof(cmd_buffer));
-
-	// Send 4 bytes to receive slave packet size
-	cmd_buffer = 0xFFFFFFFF;
-	stack_mst_write(&cmd_buffer, sizeof(cmd_buffer));
-	TRACE("switch.c: Rcv Size = %04X", cmd_buffer);
-	while(ioport_get_pin_level(SPI_IRQ1));
-}
-
-/*
-*	Write to the SPI stacking interface
-*
-*/
-void stack_mst_write(uint8_t *rx_data, uint16_t ul_size)
-{
-	uint8_t uc_pcs;
-	static uint16_t data;
-	uint8_t *p_buffer;
-	
-	p_buffer = rx_data;
-		
-	for (int i = 0; i < ul_size; i++) {
-		for(int x = 0;x<10000;x++);
-		spi_write(SPI_MASTER_BASE, p_buffer[i], 0, 0);
-		TRACE("switch.c: SPI Write - %d , %02X", i, p_buffer[i]);
-		/* Wait transfer done. */
-		while ((spi_read_status(SPI_MASTER_BASE) & SPI_SR_RDRF) == 0);
-		//spi_read(SPI_MASTER_BASE, p_buffer[i], &uc_pcs);
-		TRACE("switch.c: SPI Read - %d , %02X", i, data);
-	}
-	return;
-}
-
-/*
-*	Initialize the SPI interface to MASTER or SLAVE based on the stacking jumper
-*
-*/
-void stacking_init(bool master)
-{
-	if (master){
-		spi_slave_initialize();
-	} else {
-		spi_master_initialize();
-	}
-	return;
-}
-
-/*
-*	Initialize the SPI interface as a SLAVE
-*
-*/
-void spi_slave_initialize(void)
-{
-	NVIC_DisableIRQ(SPI_IRQn);
-	NVIC_ClearPendingIRQ(SPI_IRQn);
-	NVIC_SetPriority(SPI_IRQn, 0);
-	NVIC_EnableIRQ(SPI_IRQn);
-
-	/* Configure an SPI peripheral. */
-	spi_enable_clock(SPI_SLAVE_BASE);
-	spi_disable(SPI_SLAVE_BASE);
-	spi_reset(SPI_SLAVE_BASE);
-	spi_set_slave_mode(SPI_SLAVE_BASE);
-	spi_disable_mode_fault_detect(SPI_SLAVE_BASE);
-	spi_set_peripheral_chip_select_value(SPI_SLAVE_BASE, SPI_CHIP_SEL);
-	spi_set_clock_polarity(SPI_SLAVE_BASE, SPI_CHIP_SEL, SPI_CLK_POLARITY);
-	spi_set_clock_phase(SPI_SLAVE_BASE, SPI_CHIP_SEL, SPI_CLK_PHASE);
-	spi_set_bits_per_transfer(SPI_SLAVE_BASE, SPI_CHIP_SEL, SPI_CSR_BITS_8_BIT);
-	spi_enable_interrupt(SPI_SLAVE_BASE, SPI_IER_RDRF);
-	spi_enable(SPI_SLAVE_BASE);
-	ioport_set_pin_level(SPI_IRQ1, false);
-}
-
-/*
-*	Initialize the SPI interface as a MASTER
-*
-*/
-void spi_master_initialize(void)
-{
-	/* Configure an SPI peripheral. */
-	spi_enable_clock(SPI_MASTER_BASE);
-	spi_disable(SPI_MASTER_BASE);
-	spi_reset(SPI_MASTER_BASE);
-	spi_set_lastxfer(SPI_MASTER_BASE);
-	spi_set_master_mode(SPI_MASTER_BASE);
-	spi_disable_mode_fault_detect(SPI_MASTER_BASE);
-	spi_disable_loopback(SPI_MASTER_BASE);
-	spi_set_peripheral_chip_select_value(SPI_MASTER_BASE, SPI_CHIP_SEL);
-	//spi_set_fixed_peripheral_select(SPI_MASTER_BASE);
-	//spi_disable_peripheral_select_decode(SPI_MASTER_BASE);
-	spi_set_transfer_delay(SPI_MASTER_BASE, SPI_CHIP_SEL, SPI_DLYBS, SPI_DLYBCT);
-	spi_set_bits_per_transfer(SPI_MASTER_BASE, SPI_CHIP_SEL, SPI_CSR_BITS_8_BIT);
-	spi_set_baudrate_div(SPI_MASTER_BASE, SPI_CHIP_SEL, (sysclk_get_cpu_hz() / gs_ul_spi_clock));
-	//spi_configure_cs_behavior(SPI_MASTER_BASE, SPI_CHIP_SEL, SPI_CS_KEEP_LOW);
-	spi_set_clock_polarity(SPI_MASTER_BASE, SPI_CHIP_SEL, SPI_CLK_POLARITY);
-	spi_set_clock_phase(SPI_MASTER_BASE, SPI_CHIP_SEL, SPI_CLK_PHASE);
-
-	spi_enable(SPI_MASTER_BASE);
-}
-
-/*
-*	SPI interface IRQ handler
-*	Used to receive data from the stacking interface
-*
-*/
-void SPI_Handler(void)
-{
-	static uint16_t data;
-	uint8_t uc_pcs;
-
-	if (spi_slave_send == false)
-	{
-		if (spi_read_status(SPI_SLAVE_BASE) & SPI_SR_RDRF)
-		{
-			spi_read(SPI_SLAVE_BASE, &data, &uc_pcs);
-			//TRACE("%d - %02X", spi_data_count, data);
-
-			if (spi_state == SPI_STATE_DATA)
-			{
-				spibuffer[spi_data_count] = data;
-				spi_data_count++;
-				if (spi_data_count == spi_command_size)
-				{
-					TRACE("switch.c: %d bytes of Data received", spi_data_count);
-					uint8_t* tail_tag = (uint8_t*)(spibuffer + (int)(spi_command_size)-1);
-					uint8_t tag = *tail_tag + 1;
-					TRACE("switch.c: Tag = %d", tag);
-					gmac_write(spibuffer, spi_data_count-1, tag);
-					spi_data_count = 0;
-					spi_state = SPI_STATE_PREAMBLE;
-					return;
-				}
-			}
-
-			//	Start of Preamble
-			if (spi_state == SPI_STATE_PREAMBLE && data == 0xAA)
-			{ 
-				switch (spi_data_count)
-				{
-				case 0:
-					spi_data_count = 1;
-					break;
-				
-				case 1:
-					spi_data_count = 2;
-					break;
-			
-				case 2:
-					spi_data_count = 3;
-					break;
-				}
-			}
-		
-			//	End of Preamble
-			if (spi_state == SPI_STATE_PREAMBLE && data == 0xAB && spi_data_count == 3)
-			{
-				spi_state = SPI_STATE_COMMAND;
-				spi_data_count = 0;
-				TRACE("switch.c: Master send preamble received!");
-				return;
-			}
-			// Command bytes
-			if (spi_state == SPI_STATE_COMMAND)
-			{
-				switch(spi_data_count)
-				{
-					case 0:
-						spi_command = data;
-						spi_data_count++;
-						break;
-					
-					case 1:
-						spi_command = data<<8;
-						spi_data_count++;
-						break;
-					
-					case 2:
-						spi_command_size = data<<8;
-						spi_data_count++;
-						break;
-					case 3:
-						spi_command_size += data;
-						spi_state = SPI_STATE_DATA;
-						spi_data_count = 0;
-						TRACE("switch.c: Command received! %d - %d", spi_command, spi_command_size);
-						break;
-				}
-			}
-		
-		}	
-	return;
-	
-	} else
-	{
-		if (spi_read_status(SPI_SLAVE_BASE) & SPI_SR_RDRF)
-		{
-			spi_read(SPI_SLAVE_BASE, &data, &uc_pcs);
-			TRACE("switch.c: %d - %02X (%d)", spi_data_count, data, spi_state);
-
-			//	Start of Preamble
-			if (spi_state == SPI_STATE_PREAMBLE && data == 0xBB)
-			{
-				switch (spi_data_count)
-				{
-					case 0:
-					spi_data_count = 1;
-					spi_write(SPI_SLAVE_BASE, 0xFF, 0, 0);
-					break;
-					
-					case 1:
-					spi_data_count = 2;
-					spi_write(SPI_SLAVE_BASE, 0xFF, 0, 0);
-					break;
-					
-					case 2:
-					spi_data_count = 3;
-					spi_write(SPI_SLAVE_BASE, 0xFF, 0, 0);
-					break;
-				}
-				return;
-			}
-			
-			//	End of Preamble
-			if (spi_state == SPI_STATE_PREAMBLE && data == 0xBC && spi_data_count == 3)
-			{
-				spi_state = SPI_STATE_COMMAND;
-				spi_write(SPI_SLAVE_BASE, data, 0, 0);
-				spi_data_count = 0;
-				TRACE("switch.c: Slave send reamble received!");
-				return;
-			}
-			// Command bytes
-			if (spi_state == SPI_STATE_COMMAND)
-			{
-				switch(spi_data_count)
-				{
-					case 0:
-						spi_command = data;
-						spi_write(SPI_SLAVE_BASE, 0xFF, 0, 0);
-						spi_data_count++;
-						break;
-					
-					case 1:
-						spi_command = data<<8;
-						spi_write(SPI_SLAVE_BASE, spi_slave_send_size >> 8, 0, 0); // Size
-						spi_data_count++;
-						break;
-					
-					case 2:
-						spi_command_size = data<<8;
-						spi_write(SPI_SLAVE_BASE, spi_slave_send_size, 0, 0); // Size
-						spi_data_count++;
-						break;
-					case 3:
-						spi_command_size += data;
-						spi_state = SPI_STATE_DATA;
-						spi_write(SPI_SLAVE_BASE, 0xFF, 0, 0);
-						spi_data_count = 0;
-						//TRACE("Master fill received! %d - %d", spi_command, spi_command_size);
-						TRACE("switch.c: Packet Size = 0x%X (%d)", spi_slave_send_size, spi_slave_send_size);
-						break;
-				}
-			}
-			
-			if (spi_state == SPI_STATE_DATA)
-			{
-				ioport_set_pin_level(SPI_IRQ1, false);
-				spi_slave_send = false;
-				TRACE("switch.c: Set Slave to false!");
-				spi_state = SPI_STATE_PREAMBLE;
-				return;
-			}
-		}
-	return;		
-	}
-}
-
 /*
 *	Read from the switch registers
 *
@@ -496,7 +155,6 @@ void disableOF(void)
 	switch_write(53,0);
 	switch_write(69,0);
 	clear_flows();
-
 }
 
 /*
@@ -525,7 +183,7 @@ void update_port_stats(void)
 		phys10_port_stats[stats_rr].rx_bytes += readrxbytes(stats_rr+1);
 		phys10_port_stats[stats_rr].tx_dropped += readtxdrop(stats_rr+1);
 		phys10_port_stats[stats_rr].rx_dropped += readrxdrop(stats_rr+1);
-		phys10_port_stats[stats_rr].rx_crc_err += readrxdrop(stats_rr+1);
+		phys10_port_stats[stats_rr].rx_crc_err += readrxcrcerr(stats_rr+1);
 	}
 
 	if (OF_Version == 4)
@@ -534,7 +192,7 @@ void update_port_stats(void)
 		phys13_port_stats[stats_rr].rx_bytes += readrxbytes(stats_rr+1);
 		phys13_port_stats[stats_rr].tx_dropped += readtxdrop(stats_rr+1);
 		phys13_port_stats[stats_rr].rx_dropped += readrxdrop(stats_rr+1);
-		phys13_port_stats[stats_rr].rx_crc_err += readrxdrop(stats_rr+1);
+		phys13_port_stats[stats_rr].rx_crc_err += readrxcrcerr(stats_rr+1);
 	}
 	stats_rr++;
 	if (stats_rr == 4) stats_rr = 0;
@@ -635,7 +293,7 @@ void update_port_status(void)
 	last_port_status[0] = port_status[0];
 	last_port_status[1] = port_status[1];
 	last_port_status[2] = port_status[2];
-	last_port_status[3] = port_status[2];
+	last_port_status[3] = port_status[3];
 	// Update port status
 	port_status[0] = (switch_read(30) & 32) >> 5;
 	port_status[1] = (switch_read(46) & 32) >> 5;
@@ -658,7 +316,8 @@ void gmac_write(uint8_t *p_buffer, uint16_t ul_size, uint8_t port)
 	{
 		return;
 	}
-
+	
+	// Update port stats
 	if (port & 1) phys10_port_stats[0].tx_packets++;
 	if (port & 2) phys10_port_stats[1].tx_packets++;
 	if (port & 4) phys10_port_stats[2].tx_packets++;
@@ -667,6 +326,7 @@ void gmac_write(uint8_t *p_buffer, uint16_t ul_size, uint8_t port)
 	if (port & 2) phys13_port_stats[1].tx_packets++;
 	if (port & 4) phys13_port_stats[2].tx_packets++;
 	if (port & 8) phys13_port_stats[3].tx_packets++;
+	
 	// Add padding
 	if (ul_size < 60)
 	{
@@ -676,7 +336,9 @@ void gmac_write(uint8_t *p_buffer, uint16_t ul_size, uint8_t port)
 		last_byte = gmacbuffer + 60;
 		*last_byte = port;
 		gmac_dev_write(&gs_gmac_dev, &gmacbuffer, 61, NULL);
-	} else {
+	}
+	else
+	{
 		memcpy(&gmacbuffer,p_buffer, ul_size);
 		uint8_t *last_byte;
 		last_byte = gmacbuffer + ul_size;
@@ -684,6 +346,7 @@ void gmac_write(uint8_t *p_buffer, uint16_t ul_size, uint8_t port)
 		ul_size++; // Increase packet size by 1 to allow for the tail tag.
 		uint32_t write_size = gmac_dev_write(&gs_gmac_dev, &gmacbuffer, ul_size, NULL);
 	}
+	
 	return;
 }
 
@@ -697,7 +360,7 @@ void GMAC_Handler(void)
 }
 
 /*
-*	Switch initialisation function
+*	Switch initialization function
 *
 */
 void switch_init(void)
@@ -715,12 +378,7 @@ void switch_init(void)
 		/* Fill in GMAC options */
 		gmac_option.uc_copy_all_frame = 1;
 		gmac_option.uc_no_boardcast = 0;
-		gmac_option.uc_mac_addr[0] = Zodiac_Config.MAC_address[0];
-		gmac_option.uc_mac_addr[1] = Zodiac_Config.MAC_address[1];
-		gmac_option.uc_mac_addr[2] = Zodiac_Config.MAC_address[2];
-		gmac_option.uc_mac_addr[3] = Zodiac_Config.MAC_address[3];
-		gmac_option.uc_mac_addr[4] = Zodiac_Config.MAC_address[4];
-		gmac_option.uc_mac_addr[5] = Zodiac_Config.MAC_address[5];
+		memcpy(gmac_option.uc_mac_addr, Zodiac_Config.MAC_address, 6);
 		gs_gmac_dev.p_hw = GMAC;
 
 		/* Init KSZ8795 registers */
@@ -805,11 +463,6 @@ void switch_init(void)
 void task_switch(struct netif *netif)
 {
 	uint32_t ul_rcv_size = 0;
-	uint8_t tag = 0;
-	int8_t in_port = 0;
-			
-	// Check if the slave device has a packet to send us
-	if(masterselect == false && ioport_get_pin_level(SPI_IRQ1) && stackenabled == true) MasterStackRcv();
 
 	/* Main packet processing loop */
 	uint32_t dev_read = gmac_dev_read(&gs_gmac_dev, (uint8_t *) gs_uc_eth_buffer, sizeof(gs_uc_eth_buffer), &ul_rcv_size);
@@ -828,39 +481,31 @@ void task_switch(struct netif *netif)
 			}
 		}
 		
-		if(masterselect == false)	// Only process packets if board is set to MASTER
+		// Process packet
+		if (ul_rcv_size > 0)
 		{
-			if (ul_rcv_size > 0)
+			uint8_t* tail_tag = (uint8_t*)(gs_uc_eth_buffer + (int)(ul_rcv_size)-1);
+			uint8_t tag = *tail_tag + 1;
+			if (Zodiac_Config.OFEnabled == OF_ENABLED && Zodiac_Config.of_port[tag-1] == 1)
 			{
-				uint8_t* tail_tag = (uint8_t*)(gs_uc_eth_buffer + (int)(ul_rcv_size)-1);
-				uint8_t tag = *tail_tag + 1;
-				if (Zodiac_Config.OFEnabled == OF_ENABLED && Zodiac_Config.of_port[tag-1] == 1)
-				{
-					//MasterStackSend((uint8_t *) gs_uc_eth_buffer, ul_rcv_size);
-					phys10_port_stats[tag-1].rx_packets++;
-					phys13_port_stats[tag-1].rx_packets++;
-					ul_rcv_size--; // remove the tail first
-					nnOF_tablelookup((uint8_t *) gs_uc_eth_buffer, &ul_rcv_size, tag);
-					return;
-				} else {
-					TRACE("switch.c: %d byte received from controller", ul_rcv_size);
-					struct pbuf *p;
-					p = pbuf_alloc(PBUF_RAW, ul_rcv_size+1, PBUF_POOL);
-					memcpy(p->payload, &gs_uc_eth_buffer,(ul_rcv_size-1));
-					p->len = ul_rcv_size-1;
-					p->tot_len = ul_rcv_size-1;
-					netif->input(p, netif);
-					pbuf_free(p);
-					return;
-				}
+				phys10_port_stats[tag-1].rx_packets++;
+				phys13_port_stats[tag-1].rx_packets++;
+				ul_rcv_size--; // remove the tail first
+				nnOF_tablelookup((uint8_t *) gs_uc_eth_buffer, &ul_rcv_size, tag);
+				return;
+			}
+			else
+			{
+				TRACE("switch.c: %d byte received from controller", ul_rcv_size);
+				struct pbuf *p;
+				p = pbuf_alloc(PBUF_RAW, ul_rcv_size+1, PBUF_POOL);
+				memcpy(p->payload, &gs_uc_eth_buffer,(ul_rcv_size-1));
+				p->len = ul_rcv_size-1;
+				p->tot_len = ul_rcv_size-1;
+				netif->input(p, netif);
+				pbuf_free(p);
+				return;
 			}
-		} else
-		{
-			TRACE("switch.c: Set Slave to true!");
-			spi_slave_send = true;
-			spi_slave_send_size = ul_rcv_size;
-			ioport_set_pin_level(SPI_IRQ1, true);
-			return;
 		}
 	}
 	return;
diff --git a/ZodiacFX/src/switch.h b/ZodiacFX/src/switch.h
index 69e4c4e..e4b132a 100644
--- a/ZodiacFX/src/switch.h
+++ b/ZodiacFX/src/switch.h
@@ -32,6 +32,7 @@
 
 #define SPI_Handler     SPI_Handler
 #define SPI_IRQn        SPI_IRQn
+#define SHARED_BUFFER_LEN 2048
 
 void spi_init(void);
 void switch_init(void);
@@ -43,8 +44,10 @@ void update_port_stats(void);
 void update_port_status(void);
 void disableOF(void);
 void enableOF(void);
-void stacking_init(bool master);
-void MasterStackSend(uint8_t *p_uc_data, uint16_t ul_size);
-void MasterStackRcv(void);
 
+int readtxbytes(int port);
+int readrxbytes(int port);
+int readtxdrop(int port);
+int readrxdrop(int port);
+int readrxcrcerr(int port);
 #endif /* SWITCH_H_ */
diff --git a/ZodiacFX/src/trace.h b/ZodiacFX/src/trace.h
index 1804507..d831a3d 100644
--- a/ZodiacFX/src/trace.h
+++ b/ZodiacFX/src/trace.h
@@ -1,4 +1,5 @@
 extern bool trace;
 #define TRACE(fmt, ...) if (trace) { printf(fmt "\r\n", ## __VA_ARGS__); }
+	
 // build with this instead, to disable trace for performance.
-// #define TRACE(...) ;
+//#define TRACE(...) ;