1 files changed, 51 insertions, 4 deletions

diff --git a/ports/esp8266/modmachine.c b/ports/esp8266/modmachine.c
index 7e5f6714b..1c1d902e5 100644
--- a/ports/esp8266/modmachine.c
+++ b/ports/esp8266/modmachine.c
@@ -30,6 +30,10 @@
 
 #include "py/obj.h"
 #include "py/runtime.h"
+
+// This needs to be set before we include the RTOS headers
+#define USE_US_TIMER 1
+
 #include "extmod/machine_mem.h"
 #include "extmod/machine_signal.h"
 #include "extmod/machine_pulse.h"
@@ -160,22 +164,65 @@ STATIC void esp_timer_cb(void *arg) {
 }
 
 STATIC mp_obj_t esp_timer_init_helper(esp_timer_obj_t *self, size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum {
+        ARG_mode,
+        ARG_callback,
+        ARG_period,
+        ARG_tick_hz,
+        ARG_freq,
+    };
     static const mp_arg_t allowed_args[] = {
-//        { MP_QSTR_freq,         MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} },
-        { MP_QSTR_period,       MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0xffffffff} },
         { MP_QSTR_mode,         MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 1} },
         { MP_QSTR_callback,     MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} },
+        { MP_QSTR_period,       MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0xffffffff} },
+        { MP_QSTR_tick_hz,      MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 1000} },
+#if MICROPY_PY_BUILTINS_FLOAT
+        { MP_QSTR_freq,         MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} },
+#else
+        { MP_QSTR_freq,         MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0xffffffff} },
+#endif
     };
 
     // parse args
     mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
     mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
 
-    self->callback = args[2].u_obj;
+    self->callback = args[ARG_callback].u_obj;
     // Be sure to disarm timer before making any changes
     os_timer_disarm(&self->timer);
     os_timer_setfn(&self->timer, esp_timer_cb, self);
-    os_timer_arm(&self->timer, args[0].u_int, args[1].u_int);
+
+#if MICROPY_PY_BUILTINS_FLOAT
+    if (args[ARG_freq].u_obj != mp_const_none) {
+        mp_float_t freq = mp_obj_get_float(args[ARG_freq].u_obj);
+        if (freq < 0.001) {
+            os_timer_arm(&self->timer, (mp_int_t)(1000 / freq), args[ARG_mode].u_int);
+        } else {
+            os_timer_arm_us(&self->timer, (mp_int_t)(1000000 / freq), args[ARG_mode].u_int);
+        }
+    }
+#else
+    if (args[ARG_freq].u_int != 0xffffffff) {
+        os_timer_arm_us(&self->timer, 1000000 / args[ARG_freq].u_int, args[ARG_mode].u_int);
+    }
+#endif
+    else {
+        mp_int_t period = args[ARG_period].u_int;
+        mp_int_t hz = args[ARG_tick_hz].u_int;
+        if (hz == 1000) {
+            os_timer_arm(&self->timer, period, args[ARG_mode].u_int);
+        } else if (hz == 1000000) {
+            os_timer_arm_us(&self->timer, period, args[ARG_mode].u_int);
+        } else {
+            // Use a long long to ensure that we don't either overflow or loose accuracy
+            uint64_t period_us = (((uint64_t)period) * 1000000) / hz;
+            if (period_us < 0x80000000ull) {
+                os_timer_arm_us(&self->timer, (mp_int_t)period_us, args[ARG_mode].u_int);
+            } else {
+                os_timer_arm(&self->timer, (mp_int_t)(period_us / 1000), args[ARG_mode].u_int);
+            }
+        }
+    }
 
     return mp_const_none;
 }