arch: arm: cortex_m: move part of swap_helper to C

Asm is notoriously harder to maintain than C and requires core specific
adaptation which impairs even more the readability of the code.

This change significantly enhances the maintainability & portability of the
code at the expanse of an indirection (1 outlined function).

Signed-off-by: Wilfried Chauveau <wilfried.chauveau@arm.com>