Modify Context switch to fix IRQ and FIQ in FR.

Signed-off-by: cctsao1008 <chiacheng.tsao@gmail.com>
Modify context switch flow.

Signed-off-by: cctsao1008 <chiacheng.tsao@gmail.com>