diff --git a/ports/stm32/machine_timer.c b/ports/stm32/machine_timer.c
index c37ac87672..3f83d5ba61 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 d0a186c7d0..7e439d4e75 100644
--- a/ports/stm32/softtimer.c
+++ b/ports/stm32/softtimer.c
@@ -64,7 +64,11 @@ void soft_timer_handler(void) {
     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);
diff --git a/ports/stm32/softtimer.h b/ports/stm32/softtimer.h
index a80d9087b5..6e06bbcc65 100644
--- a/ports/stm32/softtimer.h
+++ b/ports/stm32/softtimer.h
@@ -28,15 +28,21 @@
 
 #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;