I have used the Startup file to switch between SBL and application. Inserted the code below at the beginning of the reset handler:
Possibly not the best assembly code in this world, but it works.
; ----------------------------------------------------------------------
; ----------------- Check if SBL entry is required ---------------------
; ----------------------------------------------------------------------
; constants
SblFlag_TRUE EQU 0x12345678
CRP3 EQU 0x43218765
Address_CRP EQU 0x1FC ; word
Address_SBLFLAG EQU 0x7000 ; word
AddressStart_APP EQU 0x8000
AddressEnd_BLANKCHECK EQU 0xFFFF
; Check if user code is present
; if user code not present, branch to sbl entry
LDR R0, =AddressStart_APP
LDR R1, =AddressEnd_BLANKCHECK
; loop first sector of app area
loop_start ; load first byte
LDRB R2, [R0]
; Check if byte is empty (0xFF)
CMP R2, #0xFF
; break loop and check other conditions
BNE app_present
; check if actual address equals end of sector
; subtract actual address from final address
; SUBS sets the Z flag if equal
SUBS R3, R1, R0
; if result is zero
; end loop and branch to SBL, because no code is present
BEQ enter_sbl
; increment address
ADD R0, R0, #1
; branch to next iteration (check next byte)
B loop_start
app_present ; Load registers for CRP3 check
LDR R0, =Address_CRP
; Load word value from Address_CRP
LDR R0, [R0]
LDR R1, =CRP3
; check if CRP3 is enabled
; SUBS sets the Z flag if equal
SUBS R2, R0, R1
; if CRP3 is enabled AND user code is present
; branch to application
BEQ enter_app
; Load registers for SBL flag check
LDR R0, =Address_SBLFLAG
; Load word value from Address_SBLFLAG
LDR R0, [R0]
LDR R1, =SblFlag_TRUE
; check if sbl flag is set
; SUBS sets the Z flag if equal
SUBS R2, R0, R1
; if sbl flag set, branch to sbl
; else branch to application
; (User code present check was alreay performed)
BEQ enter_sbl
; Branch to main / user application
enter_app LDR R0, =AddressStart_APP
BX R0
; branch to SBL if user code not present
; OR
; CRP3 not enabled AND SBL flag set
enter_sbl