diff --git a/ports/zephyr/CMakeLists.txt b/ports/zephyr/CMakeLists.txt
index 716642979a601..e30759e064464 100644
--- a/ports/zephyr/CMakeLists.txt
+++ b/ports/zephyr/CMakeLists.txt
@@ -46,6 +46,7 @@ set(MICROPY_SOURCE_PORT
     modutime.c
     modzephyr.c
     modzsensor.c
+    mphalport.c
     uart_core.c
     zephyr_storage.c
 )
diff --git a/ports/zephyr/boards/nucleo_wb55rg.conf b/ports/zephyr/boards/nucleo_wb55rg.conf
new file mode 100644
index 0000000000000..369dabddebf20
--- /dev/null
+++ b/ports/zephyr/boards/nucleo_wb55rg.conf
@@ -0,0 +1,2 @@
+CONFIG_CONSOLE_SUBSYS=n
+CONFIG_NETWORKING=n
diff --git a/ports/zephyr/main.c b/ports/zephyr/main.c
index 74785cc830420..f8ec0a0d27e68 100644
--- a/ports/zephyr/main.c
+++ b/ports/zephyr/main.c
@@ -45,6 +45,7 @@
 #include "py/runtime.h"
 #include "py/repl.h"
 #include "py/gc.h"
+#include "py/mphal.h"
 #include "py/stackctrl.h"
 #include "lib/utils/pyexec.h"
 #include "lib/mp-readline/readline.h"
@@ -123,6 +124,7 @@ int real_main(void) {
     mp_stack_set_limit(CONFIG_MAIN_STACK_SIZE - 512);
 
     init_zephyr();
+    mp_hal_init();
 
     #ifdef TEST
     static const char *argv[] = {"test"};
diff --git a/ports/zephyr/modmachine.c b/ports/zephyr/modmachine.c
index 107895bea0e42..4353083eebd8f 100644
--- a/ports/zephyr/modmachine.c
+++ b/ports/zephyr/modmachine.c
@@ -53,12 +53,19 @@ STATIC mp_obj_t machine_reset_cause(void) {
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_0(machine_reset_cause_obj, machine_reset_cause);
 
+STATIC mp_obj_t machine_idle(void) {
+    k_yield();
+    return mp_const_none;
+}
+STATIC MP_DEFINE_CONST_FUN_OBJ_0(machine_idle_obj, machine_idle);
+
 STATIC const mp_rom_map_elem_t machine_module_globals_table[] = {
     { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_umachine) },
     #ifdef CONFIG_REBOOT
     { MP_ROM_QSTR(MP_QSTR_reset), MP_ROM_PTR(&machine_reset_obj) },
     #endif
     { MP_ROM_QSTR(MP_QSTR_reset_cause), MP_ROM_PTR(&machine_reset_cause_obj) },
+    { MP_ROM_QSTR(MP_QSTR_idle), MP_ROM_PTR(&machine_idle_obj) },
 
     #if MICROPY_PY_MACHINE_I2C
     { MP_ROM_QSTR(MP_QSTR_I2C), MP_ROM_PTR(&machine_hard_i2c_type) },
diff --git a/ports/zephyr/mpconfigport.h b/ports/zephyr/mpconfigport.h
index 966f7f7e94a1d..162ac8a5766a2 100644
--- a/ports/zephyr/mpconfigport.h
+++ b/ports/zephyr/mpconfigport.h
@@ -96,6 +96,9 @@
 #define MICROPY_COMP_CONST (0)
 #define MICROPY_COMP_DOUBLE_TUPLE_ASSIGN (0)
 
+void mp_hal_signal_event(void);
+#define MICROPY_SCHED_HOOK_SCHEDULED mp_hal_signal_event()
+
 #define MICROPY_PY_SYS_PLATFORM "zephyr"
 
 #ifdef CONFIG_BOARD
diff --git a/ports/zephyr/mphalport.c b/ports/zephyr/mphalport.c
new file mode 100644
index 0000000000000..4f00cbd26ca67
--- /dev/null
+++ b/ports/zephyr/mphalport.c
@@ -0,0 +1,74 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2021 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "py/runtime.h"
+#include "py/mphal.h"
+
+static struct k_poll_signal wait_signal;
+static struct k_poll_event wait_events[2] = {
+    K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL,
+        K_POLL_MODE_NOTIFY_ONLY,
+        &wait_signal),
+    K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_SEM_AVAILABLE,
+        K_POLL_MODE_NOTIFY_ONLY,
+        NULL, 0),
+};
+
+void mp_hal_init(void) {
+    k_poll_signal_init(&wait_signal);
+}
+
+void mp_hal_signal_event(void) {
+    k_poll_signal_raise(&wait_signal, 0);
+}
+
+void mp_hal_wait_sem(struct k_sem *sem, uint32_t timeout_ms) {
+    mp_uint_t t0 = mp_hal_ticks_ms();
+    if (sem) {
+        k_poll_event_init(&wait_events[1], K_POLL_TYPE_SEM_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, sem);
+    }
+    for (;;) {
+        k_timeout_t wait;
+        if (timeout_ms == (uint32_t)-1) {
+            wait = K_FOREVER;
+        } else {
+            uint32_t dt = mp_hal_ticks_ms() - t0;
+            if (dt >= timeout_ms) {
+                return;
+            }
+            wait = K_MSEC(timeout_ms - dt);
+        }
+        k_poll(wait_events, sem ? 2 : 1, wait);
+        if (wait_events[0].state == K_POLL_STATE_SIGNALED) {
+            wait_events[0].signal->signaled = 0;
+            wait_events[0].state = K_POLL_STATE_NOT_READY;
+            mp_handle_pending(true);
+        } else if (sem && wait_events[1].state == K_POLL_STATE_SEM_AVAILABLE) {
+            wait_events[1].state = K_POLL_STATE_NOT_READY;
+            return;
+        }
+    }
+}
diff --git a/ports/zephyr/mphalport.h b/ports/zephyr/mphalport.h
index b3154b649bbda..63a18e138866a 100644
--- a/ports/zephyr/mphalport.h
+++ b/ports/zephyr/mphalport.h
@@ -1,6 +1,9 @@
 #include <zephyr.h>
 #include "lib/utils/interrupt_char.h"
 
+void mp_hal_init(void);
+void mp_hal_wait_sem(struct k_sem *sem, uint32_t timeout_ms);
+
 static inline mp_uint_t mp_hal_ticks_us(void) {
     return k_cyc_to_ns_floor64(k_cycle_get_32()) / 1000;
 }
@@ -21,7 +24,7 @@ static inline void mp_hal_delay_us(mp_uint_t delay) {
 }
 
 static inline void mp_hal_delay_ms(mp_uint_t delay) {
-    k_msleep(delay);
+    mp_hal_wait_sem(NULL, delay);
 }
 
 static inline uint64_t mp_hal_time_ns(void) {
diff --git a/ports/zephyr/prj.conf b/ports/zephyr/prj.conf
index c9c2e96da9beb..9e855036d2b84 100644
--- a/ports/zephyr/prj.conf
+++ b/ports/zephyr/prj.conf
@@ -13,6 +13,7 @@ CONFIG_CONSOLE_PUTCHAR_BUFSIZE=128
 CONFIG_NEWLIB_LIBC=y
 CONFIG_FPU=y
 CONFIG_MAIN_STACK_SIZE=4736
+CONFIG_POLL=y
 
 # Enable sensor subsystem (doesn't add code if not used).
 # Specific sensors should be enabled per-board.
diff --git a/ports/zephyr/prj_minimal.conf b/ports/zephyr/prj_minimal.conf
index 8b2104925ab67..806e26af77841 100644
--- a/ports/zephyr/prj_minimal.conf
+++ b/ports/zephyr/prj_minimal.conf
@@ -1,6 +1,7 @@
 CONFIG_NEWLIB_LIBC=y
 CONFIG_FPU=y
 CONFIG_MAIN_STACK_SIZE=4096
+CONFIG_POLL=y
 
 CONFIG_UART_INTERRUPT_DRIVEN=y
 CONFIG_CONSOLE_SUBSYS=y
diff --git a/ports/zephyr/src/zephyr_getchar.c b/ports/zephyr/src/zephyr_getchar.c
index d75a8a76e3d69..95d0b49959326 100644
--- a/ports/zephyr/src/zephyr_getchar.c
+++ b/ports/zephyr/src/zephyr_getchar.c
@@ -22,6 +22,8 @@
 
 extern int mp_interrupt_char;
 void mp_sched_keyboard_interrupt(void);
+void mp_hal_signal_event(void);
+void mp_hal_wait_sem(struct k_sem *sem, uint32_t timeout_ms);
 
 static struct k_sem uart_sem;
 #define UART_BUFSIZE 256
@@ -36,6 +38,7 @@ static int console_irq_input_hook(uint8_t ch)
         return 1;
     }
     if (ch == mp_interrupt_char) {
+        mp_hal_signal_event();
         mp_sched_keyboard_interrupt();
         return 1;
     } else {
@@ -49,6 +52,7 @@ static int console_irq_input_hook(uint8_t ch)
 }
 
 uint8_t zephyr_getchar(void) {
+    mp_hal_wait_sem(&uart_sem, -1);
     k_sem_take(&uart_sem, K_FOREVER);
     unsigned int key = irq_lock();
     uint8_t c = uart_ringbuf[i_get++];
diff --git a/py/mpconfig.h b/py/mpconfig.h
index 4445e9aee236b..7337994a5e85d 100644
--- a/py/mpconfig.h
+++ b/py/mpconfig.h
@@ -550,6 +550,12 @@
 #define MICROPY_VM_HOOK_RETURN
 #endif
 
+// Hook for mp_sched_schedule when a function gets scheduled on sched_queue
+// (this macro executes within an atomic section)
+#ifndef MICROPY_SCHED_HOOK_SCHEDULED
+#define MICROPY_SCHED_HOOK_SCHEDULED
+#endif
+
 // Whether to include the garbage collector
 #ifndef MICROPY_ENABLE_GC
 #define MICROPY_ENABLE_GC (0)
diff --git a/py/scheduler.c b/py/scheduler.c
index 9ff930007e595..947510c68b1c7 100644
--- a/py/scheduler.c
+++ b/py/scheduler.c
@@ -134,6 +134,7 @@ bool MICROPY_WRAP_MP_SCHED_SCHEDULE(mp_sched_schedule)(mp_obj_t function, mp_obj
         uint8_t iput = IDX_MASK(MP_STATE_VM(sched_idx) + MP_STATE_VM(sched_len)++);
         MP_STATE_VM(sched_queue)[iput].func = function;
         MP_STATE_VM(sched_queue)[iput].arg = arg;
+        MICROPY_SCHED_HOOK_SCHEDULED;
         ret = true;
     } else {
         // schedule queue is full