N210: TXRX_UHD now has bootloader #ifdef'ed into it. Safe firmware now embedded into FPGA bootloader.

Also did some cleanup of unused source files and consolidated bootloader stuff into bootloader_utils.

14 files changed, 74 insertions, 664 deletions

diff --git a/firmware/zpu/apps/blinkenlights.c b/firmware/zpu/apps/blinkenlights.c
deleted file mode 100644
index 30cb33a7f..000000000
--- a/firmware/zpu/apps/blinkenlights.c
+++ /dev/null
@@ -1,28 +0,0 @@
-/* -*- c++ -*- */
-/*
- * Copyright 2010 Ettus Research LLC
- *
- */
-
-#include "memory_map.h"
-#include <nonstdio.h>
-
-int main(int argc, char *argv[]) {
-
-	uint32_t c = 0;
-	uint8_t i = 0;
-
-	output_regs->led_src = 0;
-
-	while(1) {
-		//delay(5000000);
-		for(c=0;c<50000;c++) asm("NOP");
-		output_regs->leds = (i++ % 2) ? 0xFF : 0x00; //blink everything on that register
-	}
-
-	return 0;
-}
-
-//void delay(uint32_t t) {
-//	while(t-- != 0) asm("NOP");
-//}
diff --git a/firmware/zpu/apps/flash_test.c b/firmware/zpu/apps/flash_test.c
deleted file mode 100644
index 5b4569030..000000000
--- a/firmware/zpu/apps/flash_test.c
+++ /dev/null
@@ -1,67 +0,0 @@
-/* -*- c++ -*- */
-/*
- * Copyright 2010 Ettus Research LLC
- *
- */
-
-#include <memory_map.h>
-#include <hal_io.h>
-#include <hal_uart.h>
-#include <xilinx_s3_icap.h>
-#include <nonstdio.h>
-#include <spi_flash.h>
-#include <spi.h>
-#include <clocks.h>
-#include <string.h>
-
-//just a test to write to SPI flash and retrieve the same values.
-//uses the MOBFLEET SPI flash library
-
-void delay(uint32_t t) {
-	while(t-- != 0) asm("NOP");
-}
-
-int main(int argc, char *argv[]) {
-	uint16_t i, t;
-	uint8_t buf[260];
-	const uint8_t testdata[] = {0xDE, 0xAD, 0xBE, 0xEF, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C};
-
-  hal_disable_ints();	// In case we got here via jmp 0x0
-//	spi_init();
-  hal_uart_init();
-//	clocks_init(); //set up AD9510, enable FPGA clock @ 1x divisor
-
-	puts("SPI Flash test\n");
-	puts("Initializing SPI\n");
-
-	spif_init();
-	delay(800000);
-	puts("Erasing sector 1\n");
-	spi_flash_erase(0x00010000, 256);
-	delay(800000);
-	puts("Reading back data\n");
-	spi_flash_read(0x00010000, 256, buf);
-	delay(800000);
-
-	t=1;
-	for(i=4; i<250; i++) {
-		if(buf[i] != 0xFF) t=0;
-	}
-
-	if(!t) puts("Data was not initialized to 0xFF. Unsuccessful erase or read\n");
-	else puts("Data initialized to 0xFF, erase confirmed\n");
-
-	puts("Writing test buffer\n");
-	spi_flash_program(0x00010000, 16, testdata);
-	//memset(buf, 0, 256);
-
-	delay(800000);
-	puts("Wrote data, reading back\n");
-
-	spi_flash_read(0x00010000, 16, buf);
-
-	if(memcmp(testdata, buf, 16)) puts("Data is not the same between read and write. Unsuccessful write or read\n");
-	else puts("Successful write! Flash write correct\n");
-
-	return 0;
-}
diff --git a/firmware/zpu/apps/hardware_testbed.c b/firmware/zpu/apps/hardware_testbed.c
deleted file mode 100644
index e68e68ff7..000000000
--- a/firmware/zpu/apps/hardware_testbed.c
+++ /dev/null
@@ -1,47 +0,0 @@
-/* -*- c++ -*- */
-/*
- * Copyright 2010 Ettus Research LLC
- *
- */
-
-#include <memory_map.h>
-#include <nonstdio.h>
-#include <hal_io.h>
-#include <xilinx_s3_icap.h>
-#include <spi_flash.h>
-//#include <spi_flash_private.h>
-#include <clocks.h>
-#include <ihex.h>
-#include <bootloader_utils.h>
-#include <string.h>
-#include <hal_uart.h>
-#include <spi.h>
-
-//so this is just an evolving file used to set up and test different bits of hardware (EEPROM, clock chip, A/D, D/A, PHY)
-void delay(uint32_t t) {
-	while(t-- != 0) asm("NOP");
-}
-
-int main(int argc, char *argv[]) {
-
-	hal_disable_ints();
-  hal_uart_init();
-	spi_init();
-
-	puts("Hardware testbed. Init clocks...");
-
-	clocks_init();
-
-	//now, hopefully, we should be running at 100MHz instead of 50MHz, meaning our UART is twice as fast and we're talking at 230400.
-
-	while(1) {
-		delay(500000);
-		puts("Eat at Joe's.");
-	}
-
-
-
-
-
-	return 0;
-}
diff --git a/firmware/zpu/apps/txrx_uhd.c b/firmware/zpu/apps/txrx_uhd.c
index 8c27618e9..7a99303bf 100644
--- a/firmware/zpu/apps/txrx_uhd.c
+++ b/firmware/zpu/apps/txrx_uhd.c
@@ -39,8 +39,8 @@
 #include <string.h>
 #include <stdbool.h>
 
-#ifdef USRP2P
-#include "u2p_init.h"
+#ifdef BOOTLOADER
+#include <bootloader_utils.h>
 #endif
 
 extern uint16_t dsp0_dst_port, err0_dst_port, dsp1_dst_port;
@@ -265,7 +265,6 @@ static void handle_inp_packet(uint32_t *buff, size_t num_lines){
   handle_eth_packet(buff, num_lines);
 }
 
-
 //------------------------------------------------------------------
 
 /*
@@ -288,15 +287,23 @@ int
 main(void)
 {
   u2_init();
-#ifdef USRP2P
-  u2p_init();
-#endif
-
+#ifdef BOOTLOADER
+  spif_init();
+  set_default_mac_addr();
+  set_default_ip_addr();
+  putstr("\nUSRP N210 UDP bootloader\n");
+#else
   putstr("\nTxRx-UHD-ZPU\n");
+#endif
   print_mac_addr(ethernet_mac_addr()); newline();
   print_ip_addr(get_ip_addr()); newline();
   printf("FPGA compatibility number: %d\n", USRP2_FPGA_COMPAT_NUM);
   printf("Firmware compatibility number: %d\n", USRP2_FW_COMPAT_NUM);
+  
+#ifdef BOOTLOADER
+  //load the production FPGA image or firmware if appropriate
+  do_the_bootload_thing();
+#endif
 
   //1) register the addresses into the network stack
   register_addrs(ethernet_mac_addr(), get_ip_addr());
diff --git a/firmware/zpu/apps/uart_flash_loader.c b/firmware/zpu/apps/uart_flash_loader.c
deleted file mode 100644
index 4ec89284a..000000000
--- a/firmware/zpu/apps/uart_flash_loader.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/* -*- c++ -*- */
-/*
- * Copyright 2010 Ettus Research LLC
- *
- */
-
-//#include <stdio.h>
-#include <stdlib.h>
-#include <memory_map.h>
-#include <nonstdio.h>
-#include <hal_io.h>
-#include <hal_uart.h>
-#include <xilinx_s3_icap.h>
-#include <spi_flash.h>
-#include <spi_flash_private.h>
-//#include <clocks.h>
-#include <ihex.h>
-#include <bootloader_utils.h>
-
-//uses UART to load files to Flash in Intel HEX 16-bit format.
-//this is one example of a "safe" firmware image to be loaded by a bootloader into main RAM.
-//this CANNOT write to main RAM, since it is resident there.
-
-//Sector 0-31: Safe FPGA bootloader image
-//Sector 32-63: Production bootloader image
-//Sector 64: Production firmware image
-//Sector 127: Safe firmware image
-
-
-void uart_flash_loader(void) {
-
-	char buf[256]; //input data buffer
-	uint8_t ihx[32]; //ihex data buffer
-	uint32_t slot_offset = PROD_FW_IMAGE_LOCATION_ADDR; //initial slot offset to program to.
-	uint32_t extended_addr = 0x00000000; //extended Intel hex segment address
-
-	size_t sector_size = spi_flash_log2_sector_size();
-	ihex_record_t ihex_record;
-	ihex_record.data = ihx;
-	int i;
-
-
-	//not gonna win a turing prize for my C text parsing
-	while(1) {
-		gets(buf);
-		if(!strncmp(buf, "!SECTORSIZE", 7)) { //return the sector size in log format
-			putstr("OK ");
-			puthex8((uint32_t) sector_size); //err, this should probably be decimal for human readability. we do have itoa now...
-			putstr("\n");
-		}
-		else if(!strncmp(buf, "!SETADDR", 7)) { //set start address for programming
-			slot_offset = atol(&buf[8]);
-			puts("OK");
-//			puthex32(slot_offset);
-//			putstr("\n");
-		}
-		else if(!strncmp(buf, "!ERASE", 6)) { //erase a sector
-			uint32_t sector = atol(&buf[6]);
-			uint32_t size = 2 << (sector_size-1);
-			uint32_t addr = sector << sector_size;
-
-			spi_flash_erase(addr, size); //we DO NOT implement write protection here. it is up to the HOST PROGRAM to not issue an ERASE unless it means it.
-																	 //unfortunately the Flash cannot write-protect the segments that really matter, so we only use global write-protect
-																	 //as a means of avoiding accidental writes from runaway code / garbage on the SPI bus.
-			puts("OK");
-		}
-//can't exactly run firmware if you're already executing out of main RAM
-/*		else if(!strncmp(buf, "!RUNSFW", 7)) {
-			if(is_valid_fw_image(SAFE_FW_IMAGE_LOCATION_ADDR)) {
-				puts("OK");
-				spi_flash_read(SAFE_FW_IMAGE_LOCATION_ADDR, FW_IMAGE_SIZE_BYTES, (void *)RAM_BASE);
-				start_program(RAM_BASE);
-			} else {
-				puts("NOK");
-			}
-		}
-		else if(!strncmp(buf, "!RUNPFW", 7)) {
-			if(is_valid_fw_image(PROD_FW_IMAGE_LOCATION_ADDR)) {
-				puts("OK");
-				spi_flash_read(PROD_FW_IMAGE_LOCATION_ADDR, FW_IMAGE_SIZE_BYTES-1, (void *)RAM_BASE);
-				start_program(RAM_BASE);
-			} else {
-				puts("NOK");
-			}
-		}
-*/
-		else if(!strncmp(buf, "!RUNPFPGA", 8)) {
-			if(is_valid_fpga_image(PROD_FPGA_IMAGE_LOCATION_ADDR)) {
-				puts("OK");
-				//icap_reload_fpga(PROD_FPGA_IMAGE_LOCATION_ADDR);
-			} else {
-				puts("NOK");
-			}				
-		}
-		else if(!strncmp(buf, "!RUNSFPGA", 8)) {
-			if(is_valid_fpga_image(SAFE_FPGA_IMAGE_LOCATION_ADDR)) {
-				puts("OK");
-				//icap_reload_fpga(SAFE_FPGA_IMAGE_LOCATION_ADDR);
-			} else {
-				puts("NOK");
-			}
-		}
-		else if(!strncmp(buf, "!READ", 5)) {
-			uint32_t addr = atol(&buf[5]);
-			spi_flash_read(addr, 16, ihx);
-			for(i=0; i < 16; i++) {
-				puthex8(ihx[i]);
-			}
-			putstr("\n");
-		}
-
-		else if(!ihex_parse(buf, &ihex_record)) { //last, try to see if the input was a valid IHEX line
-			switch (ihex_record.type) {
-				case 0:
-					spi_flash_program(ihex_record.addr + slot_offset + extended_addr, ihex_record.length, ihex_record.data);
-					puts("OK");
-					break;
-				case 1:
-					//here we would expect a CRC checking or something else to take place. for now we do nothing.
-					//well, we set the extended segment addr back to 0
-					extended_addr = 0;
-					puts("DONE");
-					break;
-				case 4:
-					//set the upper 16 bits of the address
-					extended_addr = ((ihex_record.data[0] << 8) + ihex_record.data[1]) << 16;
-					puts("OK");
-					break;
-				default:
-					puts("NOK");
-			}
-		}
-		else puts("NOK");
-	} //while(1)
-}
-
-void delay(uint32_t t) {
-	while(t-- != 0) asm("NOP");
-}
-
-int main(int argc, char *argv[]) {
-	//uint8_t buf[32];
-	//int i = 0;
-
-  hal_disable_ints();	// In case we got here via jmp 0x0
-
-//	delay(10000000);
-
-	//before anything else you might want to blinkenlights just to indicate code startup to the user.
-
-  hal_uart_init();
-	spif_init();
-//	i2c_init(); //for EEPROM
-	puts("USRP2+ UART firmware loader");
-
-//	puts("Debug: loading production image, 10 bytes.");
-
-//	spi_flash_read(PROD_FW_IMAGE_LOCATION_ADDR, 10, buf);
-//	for(i=0; i < 10; i++) {
-//		puthex8(buf[i]);
-//	}
-
-	uart_flash_loader();
-
- 	//shouldn't get here. should reboot.
-	puts("shit happened.\n");
-
-	return 0;
-}
diff --git a/firmware/zpu/usrp2/CMakeLists.txt b/firmware/zpu/usrp2/CMakeLists.txt
index 75acae750..190ea9af1 100644
--- a/firmware/zpu/usrp2/CMakeLists.txt
+++ b/firmware/zpu/usrp2/CMakeLists.txt
@@ -34,6 +34,3 @@ ADD_EXECUTABLE(usrp2_txrx_uhd.elf ${CMAKE_SOURCE_DIR}/apps/txrx_uhd.c)
 TARGET_LINK_LIBRARIES(usrp2_txrx_uhd.elf libusrp2fw)
 GEN_OUTPUTS(usrp2_txrx_uhd.elf)
 
-ADD_EXECUTABLE(usrp2_blinkenlights.elf ${CMAKE_SOURCE_DIR}/apps/blinkenlights.c)
-TARGET_LINK_LIBRARIES(usrp2_blinkenlights.elf libusrp2fw)
-GEN_OUTPUTS(usrp2_blinkenlights.elf)
diff --git a/firmware/zpu/usrp2p/CMakeLists.txt b/firmware/zpu/usrp2p/CMakeLists.txt
index c7ab9abdb..976dccb9a 100644
--- a/firmware/zpu/usrp2p/CMakeLists.txt
+++ b/firmware/zpu/usrp2p/CMakeLists.txt
@@ -42,10 +42,3 @@ ADD_EXECUTABLE(usrp2p_txrx_uhd.elf ${CMAKE_SOURCE_DIR}/apps/txrx_uhd.c)
 TARGET_LINK_LIBRARIES(usrp2p_txrx_uhd.elf libusrp2pfw)
 GEN_OUTPUTS(usrp2p_txrx_uhd.elf)
 
-ADD_EXECUTABLE(usrp2p_blinkenlights.elf ${CMAKE_SOURCE_DIR}/apps/blinkenlights.c)
-TARGET_LINK_LIBRARIES(usrp2p_blinkenlights.elf libusrp2pfw)
-GEN_OUTPUTS(usrp2p_blinkenlights.elf)
-
-ADD_EXECUTABLE(usrp2p_uart_flash_loader.elf ${CMAKE_SOURCE_DIR}/apps/uart_flash_loader.c)
-TARGET_LINK_LIBRARIES(usrp2p_uart_flash_loader.elf libusrp2pfw)
-GEN_OUTPUTS(usrp2p_uart_flash_loader.elf)
diff --git a/firmware/zpu/usrp2p/bootloader/CMakeLists.txt b/firmware/zpu/usrp2p/bootloader/CMakeLists.txt
index 51e06ff54..07f234302 100644
--- a/firmware/zpu/usrp2p/bootloader/CMakeLists.txt
+++ b/firmware/zpu/usrp2p/bootloader/CMakeLists.txt
@@ -33,8 +33,10 @@ MACRO(GEN_RMI target)
 ENDMACRO(GEN_RMI)
 
 ########################################################################
-ADD_EXECUTABLE(udp_bootloader.elf udp_bootloader.c)
-TARGET_LINK_LIBRARIES(udp_bootloader.elf libusrp2pfw)
+ADD_EXECUTABLE(bootloader.elf ${CMAKE_SOURCE_DIR}/apps/txrx_uhd.c)
+ADD_DEFINITIONS(-DUSRP2P)
+ADD_DEFINITIONS(-DBOOTLOADER)
+TARGET_LINK_LIBRARIES(bootloader.elf libusrp2pfw)
 SET(GEN_OUTPUTS_BIN_SIZE 0x3fff)
-GEN_OUTPUTS(udp_bootloader.elf)
-GEN_RMI(udp_bootloader.bin)
+GEN_OUTPUTS(bootloader.elf)
+GEN_RMI(bootloader.bin)
diff --git a/firmware/zpu/usrp2p/bootloader/fpga_bootloader.c b/firmware/zpu/usrp2p/bootloader/fpga_bootloader.c
deleted file mode 100644
index f5a71a8bb..000000000
--- a/firmware/zpu/usrp2p/bootloader/fpga_bootloader.c
+++ /dev/null
@@ -1,202 +0,0 @@
-/* -*- c -*- */
-/*
- * Copyright 2009-2011 Ettus Research LLC
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-/*
- * This code is bootloader f/w for the slot 0 fpga image.  It's job is
- * to figure out which fpga image should be loaded, and then to load
- * that image from the SPI flash.  (FIXME handle retries, errors,
- * etc.)
- *
- * If the center button is down during boot, it loads firwmare
- * from 0:0 instead of its normal action.
- */
-
-#include <stdlib.h>
-#include <hal_io.h>
-#include <nonstdio.h>
-#include <mdelay.h>
-#include <quadradio/flashdir.h>
-#include <xilinx_v5_icap.h>
-#include <bootconfig.h>
-#include <bootconfig_private.h>
-#include <spi_flash.h>
-#include <string.h>
-#include <bootloader_utils.h>
-#include <hal_interrupts.h>
-
-#define VERBOSE 1
-
-#define	OUR_FPGA_IMAGE_NUMBER	0	// this code only runs in slot 0
-
-void hal_uart_init(void);
-void spif_init(void);
-void i2c_init(void);
-void bootconfig_init(void);
-
-void pic_interrupt_handler() __attribute__ ((interrupt_handler));
-
-void pic_interrupt_handler()
-{
-  // nop stub
-}
-
-static int
-flash_addr_of_fpga_slot(unsigned int fpga_slot)
-{
-  const struct flashdir *fd = get_flashdir();
-  return fd->slot[fpga_slot + fd->fpga_slot0].start << spi_flash_log2_sector_size();
-}
-
-
-/*
- * If the first 256 bytes of the image contain the string of bytes,
- * ff ff ff ff aa 99 55 66, we consider it a likely bitstream.
- */
-static bool
-looks_like_a_bitstream(unsigned int fpga_slot)
-{
-  unsigned char buf[256];
-  static const unsigned char pattern[] = {
-    0xff, 0xff, 0xff, 0xff, 0xaa, 0x99, 0x55, 0x66
-  };
-
-  // Read the first 256 bytes of the bitstream
-  spi_flash_read(flash_addr_of_fpga_slot(fpga_slot), sizeof(buf), buf);
-
-  for (int i = 0; i <= sizeof(buf) - sizeof(pattern); i++)
-    if (memcmp(pattern, &buf[i], sizeof(pattern)) == 0)
-      return true;
-
-  return false;
-}
-
-static bool
-plausible_bootconfig(bootconfig_t bc)
-{
-  // Are the fields in range?
-  if (!validate_bootconfig(bc))
-    return false;
-
-  if (!looks_like_a_bitstream(map_fpga_image_number_to_fpga_slot(bc.fpga_image_number)))
-    return false;
-
-  return true;
-}
-
-// Attempt to boot the fpga image specified in next_boot
-static void
-initial_boot_attempt(eeprom_boot_info_t *ee)
-{
-  if (ee->next_boot.fpga_image_number == OUR_FPGA_IMAGE_NUMBER){
-    load_firmware();
-    return;
-  }
-
-  ee->nattempts = 1;
-  _bc_write_eeprom_shadow();
-
-  unsigned int target_slot =
-    map_fpga_image_number_to_fpga_slot(ee->next_boot.fpga_image_number);
-  int flash_addr = flash_addr_of_fpga_slot(target_slot);
-
-  putstr("fpga_bootloader: chaining to ");
-  puthex4(ee->next_boot.fpga_image_number);
-  putchar(':');
-  puthex4(ee->next_boot.firmware_image_number);
-  newline();
-  mdelay(100);
-
-  while (1){
-    icap_reload_fpga(flash_addr);
-  }
-}
-
-int
-main(int argc, char **argv)
-{
-  hal_disable_ints();	// In case we got here via jmp 0x0
-  hal_uart_init();
-  i2c_init();
-  bootconfig_init();	// Must come after i2c_init.
-  spif_init();		// Needed for get_flashdir.
-
-  sr_leds->leds =  0xAAAA;
-
-  putstr("\n\n>>> fpga_bootloader <<<\n");
-
-  putstr("\nBOOTSTS ");
-  int bootsts = icap_read_config_reg(rBOOTSTS);
-  puthex32_nl(bootsts);
-  putstr("STAT    ");
-  int stat = icap_read_config_reg(rSTAT);
-  puthex32_nl(stat);
-
-  bool fallback =
-    ((bootsts & (BOOTSTS_VALID_0 | BOOTSTS_FALLBACK_0))
-     == (BOOTSTS_VALID_0 | BOOTSTS_FALLBACK_0));
-
-  if (fallback){
-    puts("FALLBACK_0 is set");
-    // FIXME handle fallback condition.
-  }
-
-  const struct flashdir *fd = get_flashdir();
-  if (fd == 0)
-    abort();
-
-  eeprom_boot_info_t *ee = _bc_get_eeprom_shadow();
-
-  if (VERBOSE){
-    putstr("nattempts: ");
-    puthex8_nl(ee->nattempts);
-  }
-
-  mdelay(500);	// wait for low-pass on switches
-  putstr("switches: "); puthex32_nl(readback->switches);
-
-  bool center_btn_down = (readback->switches & BTN_CENTER) != 0;
-  if (center_btn_down){
-    putstr("Center button is down!\n");
-    // Force boot of image 0:0
-    ee->next_boot = make_bootconfig(0, 0);
-  }
-
-  // if next_boot is valid, try it
-  if (plausible_bootconfig(ee->next_boot))
-    initial_boot_attempt(ee);	// no return
-
-  // if default_boot is valid, try it
-  if (plausible_bootconfig(ee->default_boot)){
-    ee->next_boot = ee->default_boot;
-    initial_boot_attempt(ee);	// no return
-  }
-
-  // If we're here, we're in trouble.  Try all of them...
-  for (int i = 0; i < 4; i++){
-    bootconfig_t bc = make_bootconfig(i, 0);
-    if (plausible_bootconfig(bc)){
-      ee->next_boot = bc;
-      initial_boot_attempt(ee);	// no return
-    }
-  }
-
-  // FIXME, try to find something we can load
-  puts("\n!!! Failed to find a valid FPGA bitstream!\n\n");
-
-  return 0;
-}
diff --git a/firmware/zpu/usrp2p/bootloader/fw_bootloader.c b/firmware/zpu/usrp2p/bootloader/fw_bootloader.c
deleted file mode 100644
index de561cf22..000000000
--- a/firmware/zpu/usrp2p/bootloader/fw_bootloader.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/* -*- c -*- */
-/*
- * Copyright 2009-2011 Ettus Research LLC
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <memory_map.h>
-#include <nonstdio.h>
-#include <stdlib.h>
-#include <bootconfig.h>
-#include <bootconfig_private.h>
-#include <bootloader_utils.h>
-#include <hal_interrupts.h>
-
-
-void hal_uart_init(void);
-void spif_init(void);
-void i2c_init(void);
-void bootconfig_init(void);
-
-void pic_interrupt_handler() __attribute__ ((interrupt_handler));
-
-void pic_interrupt_handler()
-{
-  // nop stub
-}
-
-int
-main(int argc, char **argv)
-{
-  hal_disable_ints();	// In case we got here via jmp 0x0
-  hal_uart_init();
-  i2c_init();
-  bootconfig_init();	// Must come after i2c_init.
-  spif_init();		// Needed for get_flashdir.
-
-  load_firmware();
-}
diff --git a/firmware/zpu/usrp2p/bootloader/icap_test.c b/firmware/zpu/usrp2p/bootloader/icap_test.c
deleted file mode 100644
index 5feb9d014..000000000
--- a/firmware/zpu/usrp2p/bootloader/icap_test.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/* -*- c++ -*- */
-/*
- * Copyright 2010 Ettus Research LLC
- *
- */
-
-#include <memory_map.h>
-#include <hal_io.h>
-#include <xilinx_s3_icap.h>
-#include <nonstdio.h>
-
-void delay(uint32_t t) {
-	while(t-- != 0) asm("NOP");
-}
-
-
-int main(int argc, char *argv[]) {
-	pic_init();
-	hal_uart_init();
-	puts("\nStarting delay...\n");
-
-	output_regs->leds = 0xFF;
-	delay(4000000);
-	output_regs->leds = 0x00;
-	delay(4000000);
-
-	puts("Rebooting FPGA to 0x00000000\n");
-	icap_reload_fpga((uint32_t)0x00000000);
-
-	return 0;
-}
diff --git a/firmware/zpu/usrp2p/bootloader/serial_loader_burner.c b/firmware/zpu/usrp2p/bootloader/serial_loader_burner.c
deleted file mode 100644
index 6d5d135ab..000000000
--- a/firmware/zpu/usrp2p/bootloader/serial_loader_burner.c
+++ /dev/null
@@ -1,49 +0,0 @@
-/* -*- c++ -*- */
-/*
- * Copyright 2009-2011 Ettus Research LLC
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <hal_io.h>
-#include <nonstdio.h>
-#include <mdelay.h>
-#include <gdbstub2.h>
-
-void hal_uart_init(void);
-void spif_init(void);
-
-void pic_interrupt_handler() __attribute__ ((interrupt_handler));
-
-void pic_interrupt_handler()
-{
-  // nop stub
-}
-
-int
-main(int argc, char **argv)
-{
-  hal_uart_init();
-  spif_init();
-
-  sr_leds->leds =  0;
-  mdelay(100);
-  sr_leds->leds = ~0;
-  mdelay(100);
-  sr_leds->leds =  0;
-
-  puts("\n\n>>> stage1: serial_loader_burner <<<");
-
-  gdbstub2_main_loop();
-}
diff --git a/firmware/zpu/usrp2p/bootloader_utils.c b/firmware/zpu/usrp2p/bootloader_utils.c
index 371c518a7..15e21f054 100644
--- a/firmware/zpu/usrp2p/bootloader_utils.c
+++ b/firmware/zpu/usrp2p/bootloader_utils.c
@@ -10,8 +10,13 @@
 #include <string.h>
 #include <bootloader_utils.h>
 #include <spi_flash.h>
+#include <i2c.h>
 #include <memory_map.h>
 #include <nonstdio.h>
+#include <xilinx_s3_icap.h>
+#include <mdelay.h>
+
+#define BUTTON_PUSHED ((router_status->irqs & PIC_BUTTON) ? 0 : 1)
 
 int is_valid_fpga_image(uint32_t addr) {
 //	printf("is_valid_fpga_image(): starting with addr=%x...\n", addr);
@@ -46,3 +51,51 @@ void start_program(void)
 	//all this does is tap that register
 	*((volatile uint32_t *) SR_ADDR_BLDRDONE) = 1;
 }
+
+void do_the_bootload_thing(void) {
+    bool production_image = find_safe_booted_flag();
+	set_safe_booted_flag(0); //haven't booted yet
+	
+	if(BUTTON_PUSHED) { //see memory_map.h
+		puts("Starting USRP2+ in safe mode. Loading safe firmware.");
+        return;
+	}
+	
+	if(!production_image) {
+		puts("Checking for valid production FPGA image...");
+		if(is_valid_fpga_image(PROD_FPGA_IMAGE_LOCATION_ADDR)) {
+			puts("Valid production FPGA image found. Attempting to boot.");
+			set_safe_booted_flag(1);
+			mdelay(300); //so serial output can finish
+			icap_reload_fpga(PROD_FPGA_IMAGE_LOCATION_ADDR);
+		}
+		puts("No valid production FPGA image found.\nFalling through to built-in firmware.");
+		return;
+	}
+	if(is_valid_fw_image(PROD_FW_IMAGE_LOCATION_ADDR)) {
+		puts("Valid production firmware found. Loading...");
+		spi_flash_read(PROD_FW_IMAGE_LOCATION_ADDR, FW_IMAGE_SIZE_BYTES, (void *)RAM_BASE);
+		puts("Finished loading. Starting image.");
+		mdelay(300);
+		start_program();
+		puts("ERROR: Return from main program! This should never happen!");
+		//if this happens, though, the safest thing to do is reboot the whole FPGA and start over.
+		mdelay(300);
+		icap_reload_fpga(SAFE_FPGA_IMAGE_LOCATION_ADDR);
+		return;
+	}
+	puts("No valid production firmware found. Falling through to built-in firmware.");
+	/*
+	if(is_valid_fw_image(SAFE_FW_IMAGE_LOCATION_ADDR)) {
+		spi_flash_read(SAFE_FW_IMAGE_LOCATION_ADDR, FW_IMAGE_SIZE_BYTES, (void *)RAM_BASE);
+		puts("Finished loading. Starting image.");
+		mdelay(300);
+		start_program();
+		puts("ERROR: return from main program! This should never happen!");
+		mdelay(300);
+		icap_reload_fpga(SAFE_FPGA_IMAGE_LOCATION_ADDR);
+		return;
+	}
+    puts("ERROR: no safe firmware image available. Falling through to built-in firmware.");
+    */
+}
diff --git a/firmware/zpu/usrp2p/bootloader_utils.h b/firmware/zpu/usrp2p/bootloader_utils.h
index 0f49ae6cd..d70299b88 100644
--- a/firmware/zpu/usrp2p/bootloader_utils.h
+++ b/firmware/zpu/usrp2p/bootloader_utils.h
@@ -20,3 +20,4 @@
 int is_valid_fpga_image(uint32_t addr);
 int is_valid_fw_image(uint32_t addr);
 void start_program(void);
+void do_the_bootload_thing(void);