armv7m: restore core mode after executing algorithm

Make sure we restore the core mode after executing any algorithm.

We also now check that we actually need to swap the core mode, we may
already be in the correct mode.

Change-Id: Ia48af2c108e0f9868aae241bf25f60323503f092
Signed-off-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-on: http://openocd.zylin.com/1107
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
armv7m: use ARM_MODE_THREAD core mode for algoorithm's

This makes sure we are using privileged mode when executing any loaders.

Change-Id: I18bf32ec92e1c76a66ab25e3712652bc3650b332
Signed-off-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-on: http://openocd.zylin.com/1108
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>