diff --git a/armsrc/fpgaloader.c b/armsrc/fpgaloader.c
index 933ff0fb3..a8ee2f6a7 100644
--- a/armsrc/fpgaloader.c
+++ b/armsrc/fpgaloader.c
@@ -33,6 +33,23 @@ static const uint8_t _bitparse_fixed_header[] = {0x00, 0x09, 0x0f, 0xf0, 0x0f, 0
 // Used to write the FPGA config word
 // May also be used to write to other SPI attached devices like an LCD
 //-----------------------------------------------------------------------------
+static void DisableSpi(void) {		
+	//* Reset all the Chip Select register
+    AT91C_BASE_SPI->SPI_CSR[0] = 0;
+    AT91C_BASE_SPI->SPI_CSR[1] = 0;
+    AT91C_BASE_SPI->SPI_CSR[2] = 0;
+    AT91C_BASE_SPI->SPI_CSR[3] = 0;
+
+    // Reset the SPI mode
+    AT91C_BASE_SPI->SPI_MR = 0;
+
+    // Disable all interrupts
+    AT91C_BASE_SPI->SPI_IDR = 0xFFFFFFFF;
+	
+	// SPI disable
+	AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SPIDIS;
+}
+
 void SetupSpi(int mode) {
 	// PA1	-> SPI_NCS3 chip select (MEM)
 	// PA10 -> SPI_NCS2 chip select (LCD)
@@ -56,7 +73,7 @@ void SetupSpi(int mode) {
 		GPIO_SPCK;
 
 	// Peripheral B
-	AT91C_BASE_PIOA->PIO_BSR |= GPIO_NCS2;
+	//AT91C_BASE_PIOA->PIO_BSR |= GPIO_NCS2;
 
 	//enable the SPI Peripheral clock
 	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_SPI);
@@ -66,13 +83,13 @@ void SetupSpi(int mode) {
 	switch (mode) {
 		case SPI_FPGA_MODE:
 			AT91C_BASE_SPI->SPI_MR =
-				( 0 << 24)	|	// Delay between chip selects (take default: 6 MCK periods)
-				(14 << 16)	|	// Peripheral Chip Select (selects FPGA SPI_NCS0 or PA11)
-				( 0 << 7)	|	// Local Loopback Disabled
-				( 1 << 4)	|	// Mode Fault Detection disabled
-				( 0 << 2)	|	// Chip selects connected directly to peripheral
-				( 0 << 1) 	|	// Fixed Peripheral Select
-				( 1 << 0);		// Master Mode
+				( 0 << 24)			|	// Delay between chip selects (take default: 6 MCK periods)
+				(0xE << 16)			|	// Peripheral Chip Select (selects FPGA SPI_NCS0 or PA11)
+				( 0 << 7)			|	// Local Loopback Disabled
+				AT91C_SPI_MODFDIS 	|	// Mode Fault Detection disabled
+				( 0 << 2)			|	// Chip selects connected directly to peripheral
+				AT91C_SPI_PS_FIXED	|	// Fixed Peripheral Select
+				AT91C_SPI_MSTR;		// Master Mode
 				
 			AT91C_BASE_SPI->SPI_CSR[0] =
 				( 1 << 24)	|	// Delay between Consecutive Transfers (32 MCK periods)
@@ -80,14 +97,14 @@ void SetupSpi(int mode) {
 				( 6 << 8)	|	// Serial Clock Baud Rate (baudrate = MCK/6 = 24Mhz/6 = 4M baud
 				AT91C_SPI_BITS_16 |	// Bits per Transfer (16 bits)
 				( 0 << 3)	|	// Chip Select inactive after transfer
-				( 1 << 1)	|	// Clock Phase data captured on leading edge, changes on following edge
+				AT91C_SPI_NCPHA	|	// Clock Phase data captured on leading edge, changes on following edge
 				( 0 << 0);		// Clock Polarity inactive state is logic 0
 			break;
 /*
 			case SPI_LCD_MODE:
 			AT91C_BASE_SPI->SPI_MR =
 				( 0 << 24)	|	// Delay between chip selects (take default: 6 MCK periods)
-				(11 << 16)	|	// Peripheral Chip Select (selects LCD SPI_NCS2 or PA10)
+				(0xB << 16)	|	// Peripheral Chip Select (selects LCD SPI_NCS2 or PA10)
 				( 0 << 7)	|	// Local Loopback Disabled
 				( 1 << 4)	|	// Mode Fault Detection disabled
 				( 0 << 2)	|	// Chip selects connected directly to peripheral
@@ -104,8 +121,8 @@ void SetupSpi(int mode) {
 				( 0 << 0);		// Clock Polarity inactive state is logic 0
 			break;
 */			
-		default:				// Disable SPI
-			AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SPIDIS;
+		default:
+			DisableSpi();		
 			break;
 	}
 }