19 files changed, 81 insertions, 3 deletions

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 142a63f..27cc20b 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,6 +7,17 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## [Unreleased]
 
+### Added
+
+- New assembly methods `asm::semihosting_syscall`, `asm::bootstrap`, and
+  `asm::bootload`.
+
+### Changed
+
+- `msp::write` has been deprecated in favor of `asm::bootstrap`. It was not
+  possible to use `msp::write` without causing Undefined Behavior, so all
+  existing users are encouraged to migrate.
+
 ## [v0.7.0] - 2020-11-09
 
 ### Added
diff --git a/asm/inline.rs b/asm/inline.rs
index f6a3ebb..f2014f8 100644
--- a/asm/inline.rs
+++ b/asm/inline.rs
@@ -177,11 +177,31 @@ pub unsafe fn __wfi() {
 
 /// Semihosting syscall.
 #[inline(always)]
-pub unsafe fn __syscall(mut nr: u32, arg: u32) -> u32 {
+pub unsafe fn __sh_syscall(mut nr: u32, arg: u32) -> u32 {
     asm!("bkpt #0xab", inout("r0") nr, in("r1") arg);
     nr
 }
 
+/// Set CONTROL.SPSEL to 0, write `msp` to MSP, branch to `rv`.
+#[inline(always)]
+pub unsafe fn __bootstrap(msp: u32, rv: u32) -> ! {
+    asm!(
+        "mrs {tmp}, CONTROL",
+        "bics {tmp}, {spsel}",
+        "msr CONTROL, {tmp}",
+        "isb",
+        "msr MSP, {msp}",
+        "bx {rv}",
+        // `out(reg) _` is not permitted in a `noreturn` asm! call,
+        // so instead use `in(reg) 0` and don't restore it afterwards.
+        tmp = in(reg) 0,
+        spsel = in(reg) 2,
+        msp = in(reg) msp,
+        rv = in(reg) rv,
+        options(noreturn),
+    );
+}
+
 // v7m *AND* v8m.main, but *NOT* v8m.base
 #[cfg(any(armv7m, armv8m_main))]
 pub use self::v7m::*;
diff --git a/asm/lib.rs b/asm/lib.rs
index b57642e..fc8ddc8 100644
--- a/asm/lib.rs
+++ b/asm/lib.rs
@@ -69,10 +69,11 @@ shims! {
     fn __psp_r() -> u32;
     fn __psp_w(val: u32);
     fn __sev();
-    fn __udf();
+    fn __udf() -> !;
     fn __wfe();
     fn __wfi();
-    fn __syscall(nr: u32, arg: u32) -> u32;
+    fn __sh_syscall(nr: u32, arg: u32) -> u32;
+    fn __bootstrap(msp: u32, rv: u32) -> !;
 }
 
 // v7m *AND* v8m.main, but *NOT* v8m.base
diff --git a/bin/thumbv6m-none-eabi-lto.a b/bin/thumbv6m-none-eabi-lto.a
index 74cee4d..f3858a0 100644
--- a/bin/thumbv6m-none-eabi-lto.a
+++ b/bin/thumbv6m-none-eabi-lto.a
diff --git a/bin/thumbv6m-none-eabi.a b/bin/thumbv6m-none-eabi.a
index d53c97a..571396b 100644
--- a/bin/thumbv6m-none-eabi.a
+++ b/bin/thumbv6m-none-eabi.a
diff --git a/bin/thumbv7em-none-eabi-lto.a b/bin/thumbv7em-none-eabi-lto.a
index 919e00b..51e02c6 100644
--- a/bin/thumbv7em-none-eabi-lto.a
+++ b/bin/thumbv7em-none-eabi-lto.a
diff --git a/bin/thumbv7em-none-eabi.a b/bin/thumbv7em-none-eabi.a
index b8ba53c..d6ccf53 100644
--- a/bin/thumbv7em-none-eabi.a
+++ b/bin/thumbv7em-none-eabi.a
diff --git a/bin/thumbv7em-none-eabihf-lto.a b/bin/thumbv7em-none-eabihf-lto.a
index 68dd787..cc52e63 100644
--- a/bin/thumbv7em-none-eabihf-lto.a
+++ b/bin/thumbv7em-none-eabihf-lto.a
diff --git a/bin/thumbv7em-none-eabihf.a b/bin/thumbv7em-none-eabihf.a
index 7d7349f..aea284d 100644
--- a/bin/thumbv7em-none-eabihf.a
+++ b/bin/thumbv7em-none-eabihf.a
diff --git a/bin/thumbv7m-none-eabi-lto.a b/bin/thumbv7m-none-eabi-lto.a
index a65077c..85720da 100644
--- a/bin/thumbv7m-none-eabi-lto.a
+++ b/bin/thumbv7m-none-eabi-lto.a
diff --git a/bin/thumbv7m-none-eabi.a b/bin/thumbv7m-none-eabi.a
index 59f5da2..d9cf989 100644
--- a/bin/thumbv7m-none-eabi.a
+++ b/bin/thumbv7m-none-eabi.a
diff --git a/bin/thumbv8m.base-none-eabi-lto.a b/bin/thumbv8m.base-none-eabi-lto.a
index b683c22..37e74a9 100644
--- a/bin/thumbv8m.base-none-eabi-lto.a
+++ b/bin/thumbv8m.base-none-eabi-lto.a
diff --git a/bin/thumbv8m.base-none-eabi.a b/bin/thumbv8m.base-none-eabi.a
index 87560a3..0660245 100644
--- a/bin/thumbv8m.base-none-eabi.a
+++ b/bin/thumbv8m.base-none-eabi.a
diff --git a/bin/thumbv8m.main-none-eabi-lto.a b/bin/thumbv8m.main-none-eabi-lto.a
index b4fd6fc..4312c64 100644
--- a/bin/thumbv8m.main-none-eabi-lto.a
+++ b/bin/thumbv8m.main-none-eabi-lto.a
diff --git a/bin/thumbv8m.main-none-eabi.a b/bin/thumbv8m.main-none-eabi.a
index 5ea8f33..59affcc 100644
--- a/bin/thumbv8m.main-none-eabi.a
+++ b/bin/thumbv8m.main-none-eabi.a
diff --git a/bin/thumbv8m.main-none-eabihf-lto.a b/bin/thumbv8m.main-none-eabihf-lto.a
index 87fdb8a..62fcad3 100644
--- a/bin/thumbv8m.main-none-eabihf-lto.a
+++ b/bin/thumbv8m.main-none-eabihf-lto.a
diff --git a/bin/thumbv8m.main-none-eabihf.a b/bin/thumbv8m.main-none-eabihf.a
index 29d6381..f7795d1 100644
--- a/bin/thumbv8m.main-none-eabihf.a
+++ b/bin/thumbv8m.main-none-eabihf.a
diff --git a/src/asm.rs b/src/asm.rs
index 8f23aa9..297198b 100644
--- a/src/asm.rs
+++ b/src/asm.rs
@@ -164,3 +164,48 @@ pub fn ttat(addr: *mut u32) -> u32 {
 pub unsafe fn bx_ns(addr: u32) {
     call_asm!(__bxns(addr: u32));
 }
+
+/// Semihosting syscall.
+///
+/// This method is used by cortex-m-semihosting to provide semihosting syscalls.
+#[inline]
+pub unsafe fn semihosting_syscall(nr: u32, arg: u32) -> u32 {
+    call_asm!(__sh_syscall(nr: u32, arg: u32) -> u32)
+}
+
+/// Bootstrap.
+///
+/// Clears CONTROL.SPSEL (setting the main stack to be the active stack),
+/// updates the main stack pointer to the address in `msp`, then jumps
+/// to the address in `rv`.
+///
+/// # Safety
+///
+/// `msp` and `rv` must point to valid stack memory and executable code,
+/// respectively.
+#[inline]
+pub unsafe fn bootstrap(msp: *const u32, rv: *const u32) -> ! {
+    // Ensure thumb mode is set.
+    let rv = (rv as u32) | 1;
+    let msp = msp as u32;
+    call_asm!(__bootstrap(msp: u32, rv: u32) -> !);
+}
+
+/// Bootload.
+///
+/// Reads the initial stack pointer value and reset vector from
+/// the provided vector table address, sets the active stack to
+/// the main stack, sets the main stack pointer to the new initial
+/// stack pointer, then jumps to the reset vector.
+///
+/// # Safety
+///
+/// The provided `vector_table` must point to a valid vector
+/// table, with a valid stack pointer as the first word and
+/// a valid reset vector as the second word.
+#[inline]
+pub unsafe fn bootload(vector_table: *const u32) -> ! {
+    let msp = core::ptr::read_volatile(vector_table);
+    let rv = core::ptr::read_volatile(vector_table.offset(1));
+    bootstrap(msp as *const u32, rv as *const u32);
+}
diff --git a/src/register/msp.rs b/src/register/msp.rs
index 2e8261e..bccc2ae 100644
--- a/src/register/msp.rs
+++ b/src/register/msp.rs
@@ -8,6 +8,7 @@ pub fn read() -> u32 {
 
 /// Writes `bits` to the CPU register
 #[inline]
+#[deprecated = "calling this function invokes Undefined Behavior, consider asm::bootstrap as an alternative"]
 pub unsafe fn write(bits: u32) {
     call_asm!(__msp_w(bits: u32));
 }