SAM4S Bootloader Jump Address Offset

Hi everyone, 

Apologies for asking a very similar question to those that are out there already, but this is my last hope as I have scoured Atmel and arm forums for a solution that works for me, but not found any yet,

I am trying to write a bootloader for the SAM4S, which is placed at 0x00000000 (0x00400000) in the device flash. Right now it successfully receives firmware over UART and writes it to 0x00410000.

The firmware is compiled with the linker script origin field edited to be 0x00410000.

I have gone through many iterations of the jump function, trying combinations of inline assembler instructions loading the stack pointer and program counter, to  more C based solutions using the program counter address cast to a function pointer. Some of these previous attempts caused hard faults, for which I now have a handler, so am fairly sure the latest iteration below does not fault.

The latest iteration is based upon the arm documentation (http://www.keil.com/support/docs/3913.htm) and is as follows:

static void jumpToApp( uint32_t *Address )
{
	uint32_t stackPointerAddress = Address[0]; //*(uint32_t *)(FLASH_APP_START_ADDR);
 	uint32_t resetHandlerAddress = Address[1]; //*(uint32_t *)(FLASH_APP_START_ADDR + 4);

	printf("Wait For Flash Ready \r\n");
 	while(!(EFC0->EEFC_FSR & EEFC_FSR_FRDY));

	printf("Stack Pointer: 0x%08X\r\nReset Handler: 0x%08X\r\n", stackPointerAddress, resetHandlerAddress);

	//------------------------------------------
	// From ARM docs: http://www.keil.com/support/docs/3913.htm

	//Make sure, the CPU is in privileged mode.
	if( CONTROL_nPRIV_Msk & __get_CONTROL( ) )
	{  /* not in privileged mode */
			__disable_irq();
			__set_CONTROL((__get_CONTROL( ))& 0xFFFFFFFE);  // enter priv mode
			__enable_irq();
	}

	
	// Disable IRQ
  	Disable_global_interrupt();

	//Disable SysTick and clear its exception pending bit
	SysTick->CTRL = 0 ;
	SCB->ICSR |= SCB_ICSR_PENDSTCLR_Msk ;

	//Active MSP, if the core is found to currently run with PSP
	if( CONTROL_SPSEL_Msk & __get_CONTROL( ) )
	{  /* MSP is not active */
		__set_CONTROL( __get_CONTROL( ) & ~CONTROL_SPSEL_Msk ) ;
	}

	// Load the vector table address of the user application into the SCB->VTOR register
	SCB->VTOR =  ( uint32_t )Address ;

	// Set the MSP to the value found int he User Application vector table
	__set_MSP( Address[ 0 ] ) ;

	// Set the PC to the reset vector value of the user application via a function call.
	( ( void ( * )( void ) )Address[ 1 ] )( ) ;
	
	// should never get here
	while(1)		
	{
		printf("Reboot Failed\r\n");
		delay_ms(500);
	}
	//------------------------------------------
}

The post at this link states that their issue was solved by disabling all interrupts before jumping, however after trying several methods of doing this, it does not solve my problem.

Debugging the flash sector (and via prints in the jump function), I can see that the initial stack pointer in the new vector table is 0x20003B50, and the reset vector address is 0x004104D5, which both seem reasonable to the best of my limited knowledge.

Stepping through, when execution is meant to call the new reset handler, I can see that the Program Counter is sat at 0x004104D0, which is close to, but 5 bytes behind the reset vector address it should be. The Stack Pointer also appears to be behind where it should be, reading 0x20003B28 instead of 0x20003B50.

Any help as to why the stack pointer and program counter would be offset like this when attempting to jump would be more than helpful.