diff --git a/ports/stm32/machine_timer.c b/ports/stm32/machine_timer.c
index c37ac87672ac2..3f83d5ba61ed9 100644
--- a/ports/stm32/machine_timer.c
+++ b/ports/stm32/machine_timer.c
@@ -76,10 +76,10 @@ STATIC mp_obj_t machine_timer_init_helper(machine_timer_obj_t *self, size_t n_ar
     self->expiry_ms = mp_hal_ticks_ms() + self->delta_ms;
 
     if (args[ARG_callback].u_obj != MP_OBJ_NULL) {
-        self->callback = args[ARG_callback].u_obj;
+        self->py_callback = args[ARG_callback].u_obj;
     }
 
-    if (self->callback != mp_const_none) {
+    if (self->py_callback != mp_const_none) {
         soft_timer_insert(self);
     }
 
@@ -89,8 +89,9 @@ STATIC mp_obj_t machine_timer_init_helper(machine_timer_obj_t *self, size_t n_ar
 STATIC mp_obj_t machine_timer_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
     machine_timer_obj_t *self = m_new_obj(machine_timer_obj_t);
     self->pairheap.base.type = &machine_timer_type;
+    self->flags = SOFT_TIMER_FLAG_PY_CALLBACK;
     self->delta_ms = 1000;
-    self->callback = mp_const_none;
+    self->py_callback = mp_const_none;
 
     // Get timer id (only soft timer (-1) supported at the moment)
     mp_int_t id = -1;
diff --git a/ports/stm32/softtimer.c b/ports/stm32/softtimer.c
index d0a186c7d0489..179a1b7efd2ee 100644
--- a/ports/stm32/softtimer.c
+++ b/ports/stm32/softtimer.c
@@ -26,6 +26,7 @@
 
 #include <stdint.h>
 #include "py/runtime.h"
+#include "py/mphal.h"
 #include "irq.h"
 #include "softtimer.h"
 
@@ -36,6 +37,10 @@ extern __IO uint32_t uwTick;
 
 volatile uint32_t soft_timer_next;
 
+// Soft-timer pairing heap for statically allocated (ie not via GC ) entries.
+STATIC soft_timer_entry_t *soft_timer_static_heap;
+
+// Clears out the GC-allocated soft-timer heap.
 void soft_timer_deinit(void) {
     MP_STATE_PORT(soft_timer_heap) = NULL;
 }
@@ -58,41 +63,79 @@ STATIC void soft_timer_schedule_systick(uint32_t ticks_ms) {
 }
 
 // Must be executed at IRQ_PRI_PENDSV
-void soft_timer_handler(void) {
+STATIC void soft_timer_handler_helper(soft_timer_entry_t **heap_in, bool *expiry_ms_set, uint32_t *expiry_ms) {
     uint32_t ticks_ms = uwTick;
-    soft_timer_entry_t *heap = MP_STATE_PORT(soft_timer_heap);
+    soft_timer_entry_t *heap = *heap_in;
     while (heap != NULL && TICKS_DIFF(heap->expiry_ms, ticks_ms) <= 0) {
         soft_timer_entry_t *entry = heap;
         heap = (soft_timer_entry_t *)mp_pairheap_pop(soft_timer_lt, &heap->pairheap);
-        mp_sched_schedule(entry->callback, MP_OBJ_FROM_PTR(entry));
+        if (entry->flags & SOFT_TIMER_FLAG_PY_CALLBACK) {
+            mp_sched_schedule(entry->py_callback, MP_OBJ_FROM_PTR(entry));
+        } else {
+            entry->c_callback(entry);
+        }
         if (entry->mode == SOFT_TIMER_MODE_PERIODIC) {
             entry->expiry_ms += entry->delta_ms;
             heap = (soft_timer_entry_t *)mp_pairheap_push(soft_timer_lt, &heap->pairheap, &entry->pairheap);
         }
     }
-    MP_STATE_PORT(soft_timer_heap) = heap;
-    if (heap == NULL) {
+    if (heap != NULL) {
+        if (!*expiry_ms_set || TICKS_DIFF(heap->expiry_ms, *expiry_ms) <= 0) {
+            *expiry_ms_set = true;
+            *expiry_ms = heap->expiry_ms;
+        }
+    }
+    *heap_in = heap;
+}
+
+// Must be executed at IRQ_PRI_PENDSV
+void soft_timer_handler(void) {
+    bool expiry_ms_set = false;
+    uint32_t expiry_ms;
+    soft_timer_handler_helper(&soft_timer_static_heap, &expiry_ms_set, &expiry_ms);
+    soft_timer_handler_helper(&MP_STATE_PORT(soft_timer_heap), &expiry_ms_set, &expiry_ms);
+    if (!expiry_ms_set) {
         // No more timers left, set largest delay possible
         soft_timer_next = uwTick;
     } else {
         // Set soft_timer_next so SysTick calls us back at the correct time
-        soft_timer_schedule_systick(heap->expiry_ms);
+        soft_timer_schedule_systick(expiry_ms);
     }
 }
 
-void soft_timer_insert(soft_timer_entry_t *entry) {
+STATIC void soft_timer_insert_to(soft_timer_entry_t **heap, soft_timer_entry_t *other_heap, soft_timer_entry_t *entry) {
     mp_pairheap_init_node(soft_timer_lt, &entry->pairheap);
     uint32_t irq_state = raise_irq_pri(IRQ_PRI_PENDSV);
-    MP_STATE_PORT(soft_timer_heap) = (soft_timer_entry_t *)mp_pairheap_push(soft_timer_lt, &MP_STATE_PORT(soft_timer_heap)->pairheap, &entry->pairheap);
-    if (entry == MP_STATE_PORT(soft_timer_heap)) {
-        // This new timer became the earliest one so set soft_timer_next
-        soft_timer_schedule_systick(entry->expiry_ms);
+    *heap = (soft_timer_entry_t *)mp_pairheap_push(soft_timer_lt, &(*heap)->pairheap, &entry->pairheap);
+    if (entry == *heap) {
+        // This new timer became the earliest on this heap.
+        if (other_heap == NULL || TICKS_DIFF(entry->expiry_ms, other_heap->expiry_ms) < 0) {
+            // This new timer became the earliest of all, so set soft_timer_next.
+            soft_timer_schedule_systick(entry->expiry_ms);
+        }
     }
     restore_irq_pri(irq_state);
 }
 
-void soft_timer_remove(soft_timer_entry_t *entry) {
+STATIC void soft_timer_remove_from(soft_timer_entry_t **heap, soft_timer_entry_t *entry) {
     uint32_t irq_state = raise_irq_pri(IRQ_PRI_PENDSV);
-    MP_STATE_PORT(soft_timer_heap) = (soft_timer_entry_t *)mp_pairheap_delete(soft_timer_lt, &MP_STATE_PORT(soft_timer_heap)->pairheap, &entry->pairheap);
+    *heap = (soft_timer_entry_t *)mp_pairheap_delete(soft_timer_lt, &(*heap)->pairheap, &entry->pairheap);
     restore_irq_pri(irq_state);
 }
+
+void soft_timer_static_init(soft_timer_entry_t *entry, uint16_t mode, uint32_t delta_ms, void (*cb)(soft_timer_entry_t *)) {
+    entry->flags = 0;
+    entry->mode = mode;
+    entry->expiry_ms = mp_hal_ticks_ms() + delta_ms;
+    entry->delta_ms = delta_ms;
+    entry->c_callback = cb;
+    soft_timer_insert_to(&soft_timer_static_heap, MP_STATE_VM(soft_timer_heap), entry);
+}
+
+void soft_timer_insert(soft_timer_entry_t *entry) {
+    soft_timer_insert_to(&MP_STATE_VM(soft_timer_heap), soft_timer_static_heap, entry);
+}
+
+void soft_timer_remove(soft_timer_entry_t *entry) {
+    soft_timer_remove_from(&MP_STATE_VM(soft_timer_heap), entry);
+}
diff --git a/ports/stm32/softtimer.h b/ports/stm32/softtimer.h
index a80d9087b57e3..3d56d7c983b56 100644
--- a/ports/stm32/softtimer.h
+++ b/ports/stm32/softtimer.h
@@ -28,21 +28,29 @@
 
 #include "py/pairheap.h"
 
+#define SOFT_TIMER_FLAG_PY_CALLBACK (1)
+
 #define SOFT_TIMER_MODE_ONE_SHOT (1)
 #define SOFT_TIMER_MODE_PERIODIC (2)
 
 typedef struct _soft_timer_entry_t {
     mp_pairheap_t pairheap;
-    uint32_t mode;
+    uint16_t flags;
+    uint16_t mode;
     uint32_t expiry_ms;
     uint32_t delta_ms; // for periodic mode
-    mp_obj_t callback;
+    union {
+        void (*c_callback)(struct _soft_timer_entry_t *);
+        mp_obj_t py_callback;
+    };
 } soft_timer_entry_t;
 
 extern volatile uint32_t soft_timer_next;
 
 void soft_timer_deinit(void);
 void soft_timer_handler(void);
+
+void soft_timer_static_init(soft_timer_entry_t *entry, uint16_t mode, uint32_t delta_ms, void (*cb)(soft_timer_entry_t *));
 void soft_timer_insert(soft_timer_entry_t *entry);
 void soft_timer_remove(soft_timer_entry_t *entry);