diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp
index 3aa964effe..a4abdd6e52 100644
--- a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp
@@ -69,14 +69,7 @@ bool QSPIDevice::transfer(const uint8_t *send, uint32_t send_len,
         acquire_bus(false);
         return false;
     }
-    wspi_command_t mode;
     bool ret = true;
-    mode.cmd   = _cmd_hdr.cmd;
-    mode.cfg   = _cmd_hdr.cfg;
-    mode.addr  = _cmd_hdr.addr;
-    mode.alt   = _cmd_hdr.alt;
-    mode.dummy = _cmd_hdr.dummy;
-
     if (send_len == 0 && recv_len == 0) {
         // This is just a command
         ret = !wspiCommand(qspi_devices[device_desc.bus].driver, &mode);
@@ -98,7 +91,11 @@ bool QSPIDevice::transfer(const uint8_t *send, uint32_t send_len,
 
 void QSPIDevice::set_cmd_header(const CommandHeader& cmd_hdr)
 {
-    _cmd_hdr = cmd_hdr;
+    mode.cmd   = cmd_hdr.cmd;
+    mode.cfg   = cmd_hdr.cfg;
+    mode.addr  = cmd_hdr.addr;
+    mode.alt   = cmd_hdr.alt;
+    mode.dummy = cmd_hdr.dummy;
 }
 
 
@@ -124,6 +121,28 @@ bool QSPIDevice::acquire_bus(bool acquire)
     return true;
 }
 
+
+// Enters Memory mapped or eXecution In Place or 0-4-4 mode
+bool QSPIDevice::enter_xip_mode(void** map_ptr)
+{
+    if (!acquire_bus(true)) {
+        return false;
+    }
+    wspiMapFlash(qspi_devices[device_desc.bus].driver, &mode, (uint8_t**)map_ptr);
+    acquire_bus(false);
+    return true;
+}
+
+bool QSPIDevice::exit_xip_mode()
+{
+    if (!acquire_bus(true)) {
+        return false;
+    }
+    wspiUnmapFlash(qspi_devices[device_desc.bus].driver);
+    acquire_bus(false);
+    return true;
+}
+
 /*
   return a SPIDevice given a string device name
  */
diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.h b/libraries/AP_HAL_ChibiOS/QSPIDevice.h
index 49283d8428..e3f8c3469e 100644
--- a/libraries/AP_HAL_ChibiOS/QSPIDevice.h
+++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.h
@@ -105,10 +105,14 @@ public:
 
     bool acquire_bus(bool acquire);
 
+    // Enters Memory mapped or eXecution In Place or 0-4-4 mode
+    bool enter_xip_mode(void** map_ptr) override;
+    bool exit_xip_mode() override;
+
 private:
     QSPIBus &bus;
     QSPIDesc &device_desc;
-    CommandHeader _cmd_hdr;
+    wspi_command_t mode;
 };
 
 class QSPIDeviceManager : public AP_HAL::QSPIDeviceManager