tone

Author: soburi

cores/arduino/zephyrCommon.cpp

@@ -103,9 +103,23 @@ constexpr const N max_in_list(const N max, const Head &head, const Tail &...tail
   return max_in_list((max >= head) ? max : head, tail...);
 }
 
+template <class N, class Head>
+constexpr const N sum_in_list(const N acc, const Head& head)
+{
+  return acc + head;
+}
+
+template <class N, class Head, class... Tail>
+constexpr const N sum_in_list(const N acc, const Head& head, const Tail&... tail)
+{
+  return sum_in_list(acc + head, tail...);
+}
+
 #define GPIO_NGPIOS(n, p, i) DT_PROP(DT_GPIO_CTLR_BY_IDX(n, p, i), ngpios)
 const int max_ngpios = max_in_list(
   0, DT_FOREACH_PROP_ELEM_SEP(DT_PATH(zephyr_user), digital_pin_gpios, GPIO_NGPIOS, (, )));
+const int sum_ngpios = sum_in_list(
+  0, DT_FOREACH_PROP_ELEM_SEP(DT_PATH(zephyr_user), digital_pin_gpios, GPIO_NGPIOS, (, )));
 
 /*
  * GPIO callback implementation
@@ -248,49 +262,56 @@ struct pin_timer {
   struct gpio_dt_spec spec;
 };
 
-struct k_timer arduino_pin_timers[ARRAY_SIZE(arduino_pins)];
-struct k_timer arduino_pin_timers_timeout[ARRAY_SIZE(arduino_pins)];
+struct pin_timer arduino_pin_timers[sum_ngpios];
+struct pin_timer arduino_pin_timers_timeout[sum_ngpios];
 
-void tone_expiry_cb(struct k_timer *timer) {
-  const struct gpio_dt_spec *spec = (gpio_dt_spec*)k_timer_user_data_get(timer);
-  gpio_pin_toggle_dt(spec);
+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);
 }
 
 void tone_timeout_cb(struct k_timer *timer) {
-  pin_size_t pinNumber = (pin_size_t)(uintptr_t)k_timer_user_data_get(timer);
-  noTone(pinNumber);
+  struct pin_timer *pt = (struct pin_timer *)k_timer_user_data_get(timer);
+  noTone(global_gpio_pin(pt->spec.port, pt->spec.pin));
 }
 
 void tone(pin_size_t pinNumber, unsigned int frequency, unsigned long duration) {
-  struct k_timer *timer = &arduino_pin_timers[pinNumber];
-  const struct gpio_dt_spec *spec = &arduino_pins[pinNumber];
+  struct k_timer *timer = &arduino_pin_timers[pinNumber].timer;
+  const struct gpio_dt_spec *spec = &arduino_pin_timers[pinNumber].spec;
   k_timeout_t timeout;
+  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);
 
   if (frequency == 0) {
-    gpio_pin_set_dt(spec, 0);
+    gpio_pin_set_dt(&arduino_pin_timers[pinNumber].spec, 0);
     return;
   }
 
   timeout = K_NSEC(NSEC_PER_SEC / (2 * frequency));
 
   k_timer_init(timer, tone_expiry_cb, NULL);
-  k_timer_user_data_set(timer, (void*)spec);
-  gpio_pin_set_dt(spec, 1);
+  k_timer_user_data_set(timer, &arduino_pin_timers[pinNumber]);
+  gpio_pin_set_dt(&arduino_pin_timers[pinNumber].spec, 1);
   k_timer_start(timer, timeout, timeout);
 
   if(duration > 0) {
-    timer = &arduino_pin_timers_timeout[pinNumber];
+    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, (void*)(uintptr_t)pinNumber);
+    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) {
-  k_timer_stop(&arduino_pin_timers[pinNumber]);
-  gpio_pin_set_dt(&arduino_pins[pinNumber], 0);
+  k_timer_stop(&arduino_pin_timers[pinNumber].timer);
+  gpio_pin_set_dt(&arduino_pin_timers[pinNumber].spec, 0);
 }
 
 void delay(unsigned long ms) { k_sleep(K_MSEC(ms)); }