sensor: icm45686: Add Triggers functionality

ubieda · kartben · commit 4aef791a0e73 · 2025-03-11T05:37:06.000+01:00
Only working with SENSOR_TRIG_DATA_READY so far.

Signed-off-by: Luis Ubieda &lt;luisf@croxel.com&gt;
diff --git a/drivers/sensor/tdk/icm45686/CMakeLists.txt b/drivers/sensor/tdk/icm45686/CMakeLists.txt
@@ -10,3 +10,6 @@ zephyr_library_sources(
 zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API
 	icm45686_decoder.c
 )
+zephyr_library_sources_ifdef(CONFIG_ICM45686_TRIGGER
+	icm45686_trigger.c
+)
diff --git a/drivers/sensor/tdk/icm45686/Kconfig b/drivers/sensor/tdk/icm45686/Kconfig
@@ -2,7 +2,7 @@
 # Copyright (c) 2025 CogniPilot Foundation
 # SPDX-License-Identifier: Apache-2.0
 
-config ICM45686
+menuconfig ICM45686
 	bool "ICM45686 High-precision 6-Axis Motion Tracking Device"
 	default y
 	depends on DT_HAS_INVENSENSE_ICM45686_ENABLED
@@ -11,3 +11,47 @@ config ICM45686
 	help
 	  Enable driver for ICM45686 High-precision 6-axis motion
 	  tracking device.
+
+if ICM45686
+
+choice
+	prompt "Trigger mode"
+	default ICM45686_TRIGGER_GLOBAL_THREAD
+	help
+	  Specify the type of triggering to be used by the driver
+
+config ICM45686_TRIGGER_NONE
+	bool "No trigger"
+
+config ICM45686_TRIGGER_GLOBAL_THREAD
+	bool "Use global thread"
+	depends on GPIO
+	depends on $(dt_compat_any_has_prop,$(DT_COMPAT_INVENSENSE_ICM45686),int-gpios)
+	select ICM45686_TRIGGER
+
+config ICM45686_TRIGGER_OWN_THREAD
+	bool "Use own thread"
+	depends on GPIO
+	depends on $(dt_compat_any_has_prop,$(DT_COMPAT_INVENSENSE_ICM45686),int-gpios)
+	select ICM45686_TRIGGER
+
+endchoice
+
+config ICM45686_TRIGGER
+	bool
+
+config ICM45686_THREAD_PRIORITY
+	int "Own thread priority"
+	depends on ICM45686_TRIGGER_OWN_THREAD
+	default 10
+	help
+	  The priority of the thread used for handling triggers.
+
+config ICM45686_THREAD_STACK_SIZE
+	int "Own thread stack size"
+	depends on ICM45686_TRIGGER_OWN_THREAD
+	default 1024
+	help
+	  The thread stack size.
+
+endif # ICM45686
diff --git a/drivers/sensor/tdk/icm45686/icm45686.c b/drivers/sensor/tdk/icm45686/icm45686.c
@@ -21,6 +21,7 @@
 #include "icm45686_reg.h"
 #include "icm45686_bus.h"
 #include "icm45686_decoder.h"
+#include "icm45686_trigger.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(ICM45686, CONFIG_SENSOR_LOG_LEVEL);
@@ -224,6 +225,9 @@ static void icm45686_submit(const struct device *dev, struct rtio_iodev_sqe *iod
 static DEVICE_API(sensor, icm45686_driver_api) = {
 	.sample_fetch = icm45686_sample_fetch,
 	.channel_get = icm45686_channel_get,
+#if defined(CONFIG_ICM45686_TRIGGER)
+	.trigger_set = icm45686_trigger_set,
+#endif /* CONFIG_ICM45686_TRIGGER */
 #if defined(CONFIG_SENSOR_ASYNC_API)
 	.get_decoder = icm45686_get_decoder,
 	.submit = icm45686_submit,
@@ -344,6 +348,14 @@ static int icm45686_init(const struct device *dev)
 		return err;
 	}
 
+	if (IS_ENABLED(CONFIG_ICM45686_TRIGGER)) {
+		err = icm45686_trigger_init(dev);
+		if (err) {
+			LOG_ERR("Failed to initialize triggers: %d", err);
+			return err;
+		}
+	}
+
 	LOG_DBG("Init OK");
 
 	return 0;
@@ -382,6 +394,7 @@ static int icm45686_init(const struct device *dev)
 				.lpf = DT_INST_PROP_OR(inst, gyro_lpf, 0),			   \
 			},									   \
 		},										   \
+		.int_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}),			   \
 	};											   \
 	static struct icm45686_data icm45686_data_##inst = {					   \
 		.edata.header = {								   \
diff --git a/drivers/sensor/tdk/icm45686/icm45686.h b/drivers/sensor/tdk/icm45686/icm45686.h
@@ -48,13 +48,33 @@ struct icm45686_encoded_data {
 	struct icm45686_encoded_payload payload;
 };
 
+struct icm45686_triggers {
+		struct gpio_callback cb;
+		const struct device *dev;
+		struct k_mutex lock;
+		struct {
+			struct sensor_trigger trigger;
+			sensor_trigger_handler_t handler;
+		} entry;
+#if defined(CONFIG_ICM45686_TRIGGER_OWN_THREAD)
+		K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_ICM45686_THREAD_STACK_SIZE);
+		struct k_thread thread;
+		struct k_sem sem;
+#elif defined(CONFIG_ICM45686_TRIGGER_GLOBAL_THREAD)
+		struct k_work work;
+#endif
+};
+
 struct icm45686_data {
 	struct {
 		struct rtio_iodev *iodev;
 		struct rtio *ctx;
 	} rtio;
 	/** Single-shot encoded data instance to support fetch/get API */
 	struct icm45686_encoded_data edata;
+#if defined(CONFIG_ICM45686_TRIGGER)
+	struct icm45686_triggers triggers;
+#endif /* CONFIG_ICM45686_TRIGGER */
 };
 
 struct icm45686_config {
@@ -72,6 +92,7 @@ struct icm45686_config {
 			uint8_t lpf : 3;
 		} gyro;
 	} settings;
+	struct gpio_dt_spec int_gpio;
 };
 
 static inline void icm45686_accel_ms(struct icm45686_encoded_data *edata,
diff --git a/drivers/sensor/tdk/icm45686/icm45686_reg.h b/drivers/sensor/tdk/icm45686/icm45686_reg.h
@@ -32,6 +32,11 @@
 #define REG_TEMP_DATA1_UI			0x0C
 #define REG_TEMP_DATA0_UI			0x0D
 #define REG_PWR_MGMT0				0x10
+#define REG_INT1_CONFIG0			0x16
+#define REG_INT1_CONFIG1			0x17
+#define REG_INT1_CONFIG2			0x18
+#define REG_INT1_STATUS0			0x19
+#define REG_INT1_STATUS1			0x1A
 #define REG_ACCEL_CONFIG0			0x1B
 #define REG_GYRO_CONFIG0			0x1C
 #define REG_DRIVE_CONFIG0			0x32
@@ -67,6 +72,18 @@
 
 #define REG_IPREG_SYS2_REG_131_ACCEL_LPFBW_SEL(val)	(val & BIT_MASK(3))
 
+#define REG_INT1_CONFIG0_STATUS_EN_DRDY(val)		(((val) & BIT_MASK(1)) << 2)
+#define REG_INT1_CONFIG0_STATUS_EN_FIFO_THS(val)	(((val) & BIT_MASK(1)) << 1)
+#define REG_INT1_CONFIG0_STATUS_EN_FIFO_FULL(val)	((val) & BIT_MASK(1))
+
+#define REG_INT1_CONFIG2_EN_OPEN_DRAIN(val)		(((val) & BIT_MASK(1)) << 2)
+#define REG_INT1_CONFIG2_EN_LATCH_MODE(val)		(((val) & BIT_MASK(1)) << 1)
+#define REG_INT1_CONFIG2_EN_ACTIVE_HIGH(val)		((val) & BIT_MASK(1))
+
+#define REG_INT1_STATUS0_DRDY(val)			(((val) & BIT_MASK(1)) << 2)
+#define REG_INT1_STATUS0_FIFO_THS(val)			(((val) & BIT_MASK(1)) << 1)
+#define REG_INT1_STATUS0_FIFO_FULL(val)			((val) & BIT_MASK(1))
+
 /* Misc. Defines */
 #define WHO_AM_I_ICM45686 0xE9
 
diff --git a/drivers/sensor/tdk/icm45686/icm45686_trigger.c b/drivers/sensor/tdk/icm45686/icm45686_trigger.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright (c) 2022 Intel Corporation
+ * Copyright (c) 2023 Google LLC
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/drivers/gpio.h>
+
+#include "icm45686.h"
+#include "icm45686_bus.h"
+#include "icm45686_trigger.h"
+
+#include <zephyr/logging/log.h>
+LOG_MODULE_REGISTER(ICM45686_TRIGGER, CONFIG_SENSOR_LOG_LEVEL);
+
+static void icm45686_gpio_callback(const struct device *dev,
+				   struct gpio_callback *cb,
+				   uint32_t pins)
+{
+	struct icm45686_data *data = CONTAINER_OF(cb,
+						  struct icm45686_data,
+						  triggers.cb);
+
+	ARG_UNUSED(dev);
+	ARG_UNUSED(pins);
+
+#if defined(CONFIG_ICM45686_TRIGGER_OWN_THREAD)
+	k_sem_give(&data->triggers.sem);
+#elif defined(CONFIG_ICM45686_TRIGGER_GLOBAL_THREAD)
+	k_work_submit(&data->triggers.work);
+#endif
+}
+
+static void icm45686_thread_cb(const struct device *dev)
+{
+	struct icm45686_data *data = dev->data;
+
+	(void)k_mutex_lock(&data->triggers.lock, K_FOREVER);
+
+	if (data->triggers.entry.handler) {
+		data->triggers.entry.handler(dev, &data->triggers.entry.trigger);
+	}
+
+	(void)k_mutex_unlock(&data->triggers.lock);
+}
+
+#if defined(CONFIG_ICM45686_TRIGGER_OWN_THREAD)
+
+static void icm45686_thread(void *p1, void *p2, void *p3)
+{
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+
+	struct icm45686_data *data = p1;
+
+	while (true) {
+		k_sem_take(&data->triggers.sem, K_FOREVER);
+
+		icm45686_thread_cb(data->triggers.dev);
+	}
+}
+
+#elif defined(CONFIG_ICM45686_TRIGGER_GLOBAL_THREAD)
+
+static void icm45686_work_handler(struct k_work *work)
+{
+	struct icm45686_data *data = CONTAINER_OF(work,
+						  struct icm45686_data,
+						  triggers.work);
+
+	icm45686_thread_cb(data->triggers.dev);
+}
+
+#endif
+
+static int icm45686_enable_drdy(const struct device *dev, bool enable)
+{
+	uint8_t val;
+	int err;
+
+	err = icm45686_bus_read(dev, REG_INT1_CONFIG0, &val, 1);
+	if (err) {
+		return err;
+	}
+
+	val &= ~REG_INT1_CONFIG0_STATUS_EN_DRDY(true);
+	err = icm45686_bus_write(dev, REG_INT1_CONFIG0, &val, 1);
+	if (err) {
+		return err;
+	}
+
+	if (enable) {
+		val |= REG_INT1_CONFIG0_STATUS_EN_DRDY(true);
+	}
+
+	return icm45686_bus_write(dev, REG_INT1_CONFIG0, &val, 1);
+}
+
+int icm45686_trigger_set(const struct device *dev,
+			 const struct sensor_trigger *trig,
+			 sensor_trigger_handler_t handler)
+{
+	int err = 0;
+	struct icm45686_data *data = dev->data;
+
+	(void)k_mutex_lock(&data->triggers.lock, K_FOREVER);
+
+	switch (trig->type) {
+	case SENSOR_TRIG_DATA_READY:
+		data->triggers.entry.trigger = *trig;
+		data->triggers.entry.handler = handler;
+
+		if (handler) {
+			/* Enable data ready interrupt */
+			err = icm45686_enable_drdy(dev, true);
+		} else {
+			/* Disable data ready interrupt */
+			err = icm45686_enable_drdy(dev, false);
+		}
+		break;
+	default:
+		err = -ENOTSUP;
+		break;
+	}
+
+	(void)k_mutex_unlock(&data->triggers.lock);
+
+	return err;
+}
+
+int icm45686_trigger_init(const struct device *dev)
+{
+	const struct icm45686_config *cfg = dev->config;
+	struct icm45686_data *data = dev->data;
+	uint8_t val = 0;
+	int err;
+
+	err = k_mutex_init(&data->triggers.lock);
+	__ASSERT_NO_MSG(!err);
+
+	/** Needed to get back the device handle from the callback context */
+	data->triggers.dev = dev;
+
+#if defined(CONFIG_ICM45686_TRIGGER_OWN_THREAD)
+
+	err = k_sem_init(&data->triggers.sem, 0, 1);
+	__ASSERT_NO_MSG(!err);
+
+	(void)k_thread_create(&data->triggers.thread,
+			      data->triggers.thread_stack,
+			      K_KERNEL_STACK_SIZEOF(data->triggers.thread_stack),
+			      icm45686_thread,
+			      data,
+			      NULL,
+			      NULL,
+			      K_PRIO_COOP(CONFIG_ICM45686_THREAD_PRIORITY),
+			      0,
+			      K_NO_WAIT);
+
+#elif defined(CONFIG_ICM45686_TRIGGER_GLOBAL_THREAD)
+	k_work_init(&data->triggers.work, icm45686_work_handler);
+#endif
+
+	if (!cfg->int_gpio.port) {
+		LOG_ERR("Interrupt GPIO not supplied");
+		return -ENODEV;
+	}
+
+	if (!gpio_is_ready_dt(&cfg->int_gpio)) {
+		LOG_ERR("Interrupt GPIO not ready");
+		return -ENODEV;
+	}
+
+	err = gpio_pin_configure_dt(&cfg->int_gpio, GPIO_INPUT);
+	if (err) {
+		LOG_ERR("Failed to configure interrupt GPIO");
+		return -EIO;
+	}
+
+	gpio_init_callback(&data->triggers.cb,
+			icm45686_gpio_callback,
+			BIT(cfg->int_gpio.pin));
+
+	err = gpio_add_callback(cfg->int_gpio.port, &data->triggers.cb);
+	if (err) {
+		LOG_ERR("Failed to add interrupt callback");
+		return -EIO;
+	}
+
+	err = gpio_pin_interrupt_configure_dt(&cfg->int_gpio,
+					      GPIO_INT_EDGE_TO_ACTIVE);
+	if (err) {
+		LOG_ERR("Failed to configure interrupt");
+	}
+
+	err = icm45686_bus_write(dev, REG_INT1_CONFIG0, &val, 1);
+	if (err) {
+		LOG_ERR("Failed to disable all INTs");
+	}
+
+	val = REG_INT1_CONFIG2_EN_OPEN_DRAIN(false) |
+	      REG_INT1_CONFIG2_EN_ACTIVE_HIGH(true);
+
+	err = icm45686_bus_write(dev, REG_INT1_CONFIG2, &val, 1);
+	if (err) {
+		LOG_ERR("Failed to configure INT as push-pull: %d", err);
+	}
+
+	return err;
+}
diff --git a/drivers/sensor/tdk/icm45686/icm45686_trigger.h b/drivers/sensor/tdk/icm45686/icm45686_trigger.h

Original file line number	Diff line number	Diff line change
`@@ -10,3 +10,6 @@ zephyr_library_sources(`
`10`	`10`	`zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API`
`11`	`11`	`icm45686_decoder.c`
`12`	`12`	`)`
	`13`	`+zephyr_library_sources_ifdef(CONFIG_ICM45686_TRIGGER`
	`14`	`+ icm45686_trigger.c`
	`15`	`+)`