diff --git a/include/cortex.h b/include/cortex.h
new file mode 100644
index 0000000..1586c8b
--- /dev/null
+++ b/include/cortex.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of the libopeninv project.
+ *
+ * Copyright (C) 2025 David J. Fiddes <D.J@fiddes.net>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CORTEX_H
+#define CORTEX_H
+
+#include <stdint.h>
+
+/** Do not mask any interrupts */
+#define CM_BASEPRI_ENABLE_INTERRUPTS 0
+
+/** \brief Set the BASEPRI register to the given priority level */
+inline __attribute__((always_inline)) void cm_set_basepriority(uint32_t new_priority)
+{
+    __asm__ volatile("msr basepri, %0 " ::"r"(new_priority) : "memory");
+}
+
+#endif // CORTEX_H
diff --git a/include/errormessage.h b/include/errormessage.h
index b9f0560..76269e8 100644
--- a/include/errormessage.h
+++ b/include/errormessage.h
@@ -48,6 +48,8 @@ class ErrorMessage
       static void PrintAllErrors();
       static void PrintNewErrors();
       static ERROR_MESSAGE_NUM GetLastError();
+      static ERROR_MESSAGE_NUM GetErrorNum(uint8_t index);
+      static uint32_t GetErrorTime(uint8_t index);
    protected:
    private:
       static void PrintError(uint32_t time, ERROR_MESSAGE_NUM err);
diff --git a/src/cansdo.cpp b/src/cansdo.cpp
index 3241c6d..8e55263 100644
--- a/src/cansdo.cpp
+++ b/src/cansdo.cpp
@@ -18,6 +18,7 @@
  */
 #include "cansdo.h"
 #include "my_math.h"
+#include "errormessage.h"
 
 #define SDO_REQ_ID_BASE       0x600U
 #define SDO_REP_ID_BASE       0x580U
@@ -28,6 +29,9 @@
 #define SDO_INDEX_MAP_RX      0x3001
 #define SDO_INDEX_MAP_RD      0x3100
 #define SDO_INDEX_STRINGS     0x5001
+#define SDO_INDEX_ERROR_NUM   0x5002
+#define SDO_INDEX_ERROR_TIME  0x5003
+
 
 #define PRINT_BUF_ENQUEUE(c)  printBuffer[(printByteIn++) & (sizeof(printBuffer) - 1)] = c
 #define PRINT_BUF_DEQUEUE()   printBuffer[(printByteOut++) & (sizeof(printBuffer) - 1)]
@@ -196,6 +200,32 @@ void CanSdo::ProcessSDO(uint32_t data[2])
    {
       ReadOrDeleteCanMap(sdo);
    }
+   else if (sdo->index == SDO_INDEX_ERROR_NUM)
+   {
+      if (sdo->cmd == SDO_READ)
+      {
+         sdo->data = ErrorMessage::GetErrorNum(sdo->subIndex);
+         sdo->cmd = SDO_READ_REPLY;
+      }
+      else
+      {
+         sdo->cmd = SDO_ABORT;
+         sdo->data = SDO_ERR_INVIDX;
+      }
+   }
+   else if (sdo->index == SDO_INDEX_ERROR_TIME)
+   {
+      if (sdo->cmd == SDO_READ)
+      {
+         sdo->data = ErrorMessage::GetErrorTime(sdo->subIndex);
+         sdo->cmd = SDO_READ_REPLY;
+      }
+      else
+      {
+         sdo->cmd = SDO_ABORT;
+         sdo->data = SDO_ERR_INVIDX;
+      }
+   }
    else
    {
       if (!ProcessSpecialSDOObjects(sdo))
diff --git a/src/errormessage.cpp b/src/errormessage.cpp
index 313fd72..20195a9 100644
--- a/src/errormessage.cpp
+++ b/src/errormessage.cpp
@@ -106,6 +106,27 @@ ERROR_MESSAGE_NUM ErrorMessage::GetLastError()
    return lastError;
 }
 
+ERROR_MESSAGE_NUM ErrorMessage::GetErrorNum(uint8_t index)
+{
+   if (index < ERROR_BUF_SIZE)
+   {
+      if (errorBuffer[index].time > 0)
+         return errorBuffer[index].msg;
+   }
+
+   return ERROR_NONE;
+}
+
+uint32_t ErrorMessage::GetErrorTime(uint8_t index)
+{
+   if (index < ERROR_BUF_SIZE)
+   {
+      return errorBuffer[index].time;
+   }
+
+   return 0;
+}
+
 /** Print all errors currently in error memory */
 void ErrorMessage::PrintAllErrors()
 {
diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp
index 16d0435..f35f4f8 100644
--- a/src/stm32_can.cpp
+++ b/src/stm32_can.cpp
@@ -28,6 +28,7 @@
 #include <libopencm3/cm3/common.h>
 #include <libopencm3/cm3/nvic.h>
 #include "stm32_can.h"
+#include "cortex.h"
 
 #define MAX_INTERFACES        2
 #define IDS_PER_BANK          4
@@ -37,6 +38,10 @@
 #define CAN_PERIPH_SPEED 36
 #endif // CAN_PERIPH_SPEED
 
+#ifndef CAN_MAX_IRQ_PRIORITY
+#warning "CAN_MAX_IRQ_PRIORITY the highest interrupt priority users of the CAN interface may use is not defined"
+#endif // CAN_MAX_IRQ_PRIORITY
+
 struct CANSPEED
 {
    uint32_t ts1;
@@ -198,6 +203,8 @@ void Stm32Can::SetBaudrate(enum baudrates baudrate)
  */
 void Stm32Can::Send(uint32_t canId, uint32_t data[2], uint8_t len)
 {
+   cm_set_basepriority(CAN_MAX_IRQ_PRIORITY);
+
    can_disable_irq(canDev, CAN_IER_TMEIE);
 
    if (can_transmit(canDev, canId, canId > 0x7FF, false, len, (uint8_t*)data) < 0 && sendCnt < SENDBUFFER_LEN)
@@ -214,8 +221,9 @@ void Stm32Can::Send(uint32_t canId, uint32_t data[2], uint8_t len)
    {
       can_enable_irq(canDev, CAN_IER_TMEIE);
    }
-}
 
+   cm_set_basepriority(CM_BASEPRI_ENABLE_INTERRUPTS);
+}
 
 Stm32Can* Stm32Can::GetInterface(int index)
 {