tone

Author: soburi

cores/arduino/zephyrCommon.cpp

@@ -260,53 +260,59 @@ PinStatus digitalRead(pin_size_t pinNumber) {
 struct pin_timer {
   struct k_timer timer;
   struct gpio_dt_spec spec;
+  uint32_t count;
 };
 
-struct pin_timer arduino_pin_timers[sum_ngpios];
-struct pin_timer arduino_pin_timers_timeout[sum_ngpios];
+struct pin_timer arduino_pin_timers[20];
 
 void tone_expiry_cb(struct k_timer *timer)
 {
-  struct pin_timer *pt = (struct pin_timer *)k_timer_user_data_get(timer);
-  gpio_pin_toggle_dt(&pt->spec);
-}
+  struct pin_timer *pt = CONTAINER_OF(timer, struct pin_timer, timer);
 
-void tone_timeout_cb(struct k_timer *timer) {
-  struct pin_timer *pt = (struct pin_timer *)k_timer_user_data_get(timer);
-  noTone(global_gpio_pin(pt->spec.port, pt->spec.pin));
+  if (pt->count == 0) {
+    k_timer_stop(timer);
+    gpio_pin_set_dt(&pt->spec, 0);
+  } else {
+    gpio_pin_toggle_dt(&pt->spec);
+    if (pt->count != UINT32_MAX) {
+      pt->count--;
+    }
+  }
 }
 
 void tone(pin_size_t pinNumber, unsigned int frequency, unsigned long duration) {
   struct k_timer *timer = &arduino_pin_timers[pinNumber].timer;
   const struct gpio_dt_spec *spec = &arduino_pin_timers[pinNumber].spec;
   k_timeout_t timeout;
+  uint32_t count;
+
   arduino_pin_timers[pinNumber].spec.port = local_gpio_port(pinNumber);
   arduino_pin_timers[pinNumber].spec.pin = local_gpio_pin(pinNumber);
   arduino_pin_timers[pinNumber].spec.dt_flags = local_gpio_dt_flags(pinNumber);
 
   pinMode(pinNumber, OUTPUT);
+  k_timer_stop(&arduino_pin_timers[pinNumber].timer);
 
   if (frequency == 0) {
     gpio_pin_set_dt(&arduino_pin_timers[pinNumber].spec, 0);
     return;
   }
 
+  if (duration == 0) {
+    count = UINT32_MAX;
+  } else {
+    count = ((uint64_t)duration * (uint64_t)frequency) / 500ULL;
+  }
+
   timeout = K_NSEC(NSEC_PER_SEC / (2 * frequency));
+  if (timeout.ticks == 0) {
+    timeout.ticks = 1;
+  }
+  arduino_pin_timers[pinNumber].count = count;
 
   k_timer_init(timer, tone_expiry_cb, NULL);
-  k_timer_user_data_set(timer, &arduino_pin_timers[pinNumber]);
-  gpio_pin_set_dt(&arduino_pin_timers[pinNumber].spec, 1);
+  gpio_pin_set_dt(&arduino_pin_timers[pinNumber].spec, 0);
   k_timer_start(timer, timeout, timeout);
-
-  if(duration > 0) {
-    timer = &arduino_pin_timers_timeout[pinNumber].timer;
-    arduino_pin_timers_timeout[pinNumber].spec.port = local_gpio_port(pinNumber);
-    arduino_pin_timers_timeout[pinNumber].spec.pin = local_gpio_pin(pinNumber);
-    arduino_pin_timers_timeout[pinNumber].spec.dt_flags = local_gpio_dt_flags(pinNumber);
-    k_timer_init(timer, tone_timeout_cb, NULL);
-    k_timer_user_data_set(timer, &arduino_pin_timers_timeout[pinNumber]);
-    k_timer_start(timer, K_MSEC(duration), K_NO_WAIT);
-  }
 }
 
 void noTone(pin_size_t pinNumber) {