At present the trap handler returns to hardcoded M-mode/S-mode. Change to returning to previous privilege level instead. Signed-off-by: Bin Meng <bmeng.cn@gmail.com> Reviewed-by: Lukas Auer <lukas.auer@aisec.fraunhofer.de> Reviewed-by: Anup Patel <anup@brainfault.org>