drivers/sensor: lis2dw12: add threshold interrupt support

Add optional threshold interrupt support.
Implemented using SENSOR_TRIG_THRESHOLD sensor trigger type.
The features can be optionally enabled through Kconfig,
or disabled for smaller code size.

Signed-off-by: Maxime Vincent <[email protected]>

Author: maximevince

drivers/sensor/lis2dw12/Kconfig

@@ -59,4 +59,16 @@ config LIS2DW12_TAP
 
 endif # LIS2DW12_TRIGGER
 
+config LIS2DW12_THRESHOLD
+	bool "Wakeup threshold trigger (via interrupt)"
+	help
+	  Enable the wakeup threshold trigger feature.
+	  The wake-up interrupt signal is generated if a certain number of
+	  consecutive data exceed the configured threshold (config in DT).
+	  The threshold is applied to both positive and negative data: for
+	  a wake-up interrupt generation at least one of the three axes must
+	  be bigger than the threshold. See ST AN5038 for more details about
+	  the feature.
+
+
 endif # LIS2DW12

drivers/sensor/lis2dw12/lis2dw12.c

@@ -11,6 +11,7 @@
 #define DT_DRV_COMPAT st_lis2dw12
 
 #include <init.h>
+#include <stdlib.h>
 #include <sys/__assert.h>
 #include <sys/byteorder.h>
 #include <logging/log.h>
@@ -157,11 +158,102 @@ static int lis2dw12_config(const struct device *dev, enum sensor_channel chan,
 	return -ENOTSUP;
 }
 
+
+static inline int32_t sensor_ms2_to_mg(const struct sensor_value *ms2)
+{
+	int64_t nano_ms2 = (ms2->val1 * 1000000LL + ms2->val2) * 1000LL;
+
+	if (nano_ms2 > 0) {
+		return (nano_ms2 + SENSOR_G / 2) / SENSOR_G;
+	} else {
+		return (nano_ms2 - SENSOR_G / 2) / SENSOR_G;
+	}
+}
+
+#if CONFIG_LIS2DW12_THRESHOLD
+
+/* Converts a lis2dw12_fs_t range to its value in milli-g
+ * Range can be 2/4/8/16G
+ */
+#define FS_RANGE_TO_MG(fs_range)	((2U << fs_range) * 1000U)
+
+/* Converts a range in mg to the lsb value for the WK_THS register
+ * For the reg value: 1 LSB = 1/64 of FS
+ * Range can be 2/4/8/16G
+ */
+#define MG_TO_WK_THS_LSB(range_mg)	(range_mg / 64)
+
+/* Calculates the WK_THS reg value
+ * from the threshold in mg and the lsb value in mg
+ * with correct integer rounding
+ */
+#define THRESHOLD_MG_TO_WK_THS_REG(thr_mg, lsb_mg) \
+	((thr_mg + (lsb_mg / 2)) / lsb_mg)
+
+static int lis2dw12_attr_set_thresh(const struct device *dev,
+					enum sensor_channel chan,
+					enum sensor_attribute attr,
+					const struct sensor_value *val)
+{
+	uint8_t reg;
+	size_t ret;
+	int lsb_mg;
+	const struct lis2dw12_device_config *cfg = dev->config;
+	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
+
+	LOG_DBG("%s on channel %d", __func__, chan);
+
+	/* can only be set for all directions at once */
+	if (chan != SENSOR_CHAN_ACCEL_XYZ) {
+		return -EINVAL;
+	}
+
+	/* Configure wakeup threshold threshold. */
+	lis2dw12_fs_t range;
+	int err = lis2dw12_full_scale_get(ctx, &range);
+
+	if (err) {
+		return err;
+	}
+
+	uint32_t thr_mg = abs(sensor_ms2_to_mg(val));
+
+	/* Check maximum value: depends on current FS value */
+	if (thr_mg >= FS_RANGE_TO_MG(range)) {
+		return -EINVAL;
+	}
+
+	/* The threshold is applied to both positive and negative data:
+	 * for a wake-up interrupt generation at least one of the three axes must be
+	 * bigger than the threshold.
+	 */
+	lsb_mg = MG_TO_WK_THS_LSB(FS_RANGE_TO_MG(range));
+	reg = THRESHOLD_MG_TO_WK_THS_REG(thr_mg, lsb_mg);
+
+	LOG_DBG("Threshold %d mg -> fs: %u mg -> reg = %d LSBs",
+			thr_mg, FS_RANGE_TO_MG(range), reg);
+	ret = 0;
+
+	return lis2dw12_wkup_threshold_set(ctx, reg);
+}
+#endif
+
 static int lis2dw12_attr_set(const struct device *dev,
 			      enum sensor_channel chan,
 			      enum sensor_attribute attr,
 			      const struct sensor_value *val)
 {
+#if CONFIG_LIS2DW12_THRESHOLD
+	switch (attr) {
+	case SENSOR_ATTR_UPPER_THRESH:
+	case SENSOR_ATTR_LOWER_THRESH:
+		return lis2dw12_attr_set_thresh(dev, chan, attr, val);
+	default:
+		/* Do nothing */
+		break;
+	}
+#endif
+
 	switch (chan) {
 	case SENSOR_CHAN_ACCEL_X:
 	case SENSOR_CHAN_ACCEL_Y:

drivers/sensor/lis2dw12/lis2dw12.h

@@ -101,6 +101,9 @@ struct lis2dw12_data {
 	sensor_trigger_handler_t tap_handler;
 	sensor_trigger_handler_t double_tap_handler;
 #endif /* CONFIG_LIS2DW12_TAP */
+#ifdef CONFIG_LIS2DW12_THRESHOLD
+	sensor_trigger_handler_t threshold_handler;
+#endif /* CONFIG_LIS2DW12_THRESHOLD */
 #if defined(CONFIG_LIS2DW12_TRIGGER_OWN_THREAD)
 	K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_LIS2DW12_THREAD_STACK_SIZE);
 	struct k_thread thread;

drivers/sensor/lis2dw12/lis2dw12_trigger.c

@@ -67,6 +67,21 @@ static int lis2dw12_enable_int(const struct device *dev,
 		return lis2dw12_pin_int1_route_set(ctx,
 				&int_route.ctrl4_int1_pad_ctrl);
 #endif /* CONFIG_LIS2DW12_TAP */
+#ifdef CONFIG_LIS2DW12_THRESHOLD
+	/**
+	 * Trigger fires when channel reading transitions configured
+	 * thresholds.  The thresholds are configured via the @ref
+	 * SENSOR_ATTR_LOWER_THRESH and @ref SENSOR_ATTR_UPPER_THRESH
+	 * attributes.
+	 */
+	case SENSOR_TRIG_THRESHOLD:
+		LOG_DBG("Setting int1_wu: %d\n", enable);
+		lis2dw12_pin_int1_route_get(ctx,
+				&int_route.ctrl4_int1_pad_ctrl);
+		int_route.ctrl4_int1_pad_ctrl.int1_wu = enable;
+		return lis2dw12_pin_int1_route_set(ctx,
+						   &int_route.ctrl4_int1_pad_ctrl);
+#endif
 	default:
 		LOG_ERR("Unsupported trigger interrupt route %d", type);
 		return -ENOTSUP;
@@ -121,6 +136,14 @@ int lis2dw12_trigger_set(const struct device *dev,
 		lis2dw12->double_tap_handler = handler;
 		return lis2dw12_enable_int(dev, SENSOR_TRIG_DOUBLE_TAP, state);
 #endif /* CONFIG_LIS2DW12_TAP */
+#ifdef CONFIG_LIS2DW12_THRESHOLD
+	case SENSOR_TRIG_THRESHOLD:
+	{
+		LOG_DBG("Set trigger %d (handler: %p)\n", trig->type, handler);
+		lis2dw12->threshold_handler = handler;
+		return lis2dw12_enable_int(dev, SENSOR_TRIG_THRESHOLD, state);
+	}
+#endif
 	default:
 		LOG_ERR("Unsupported sensor trigger");
 		return -ENOTSUP;
@@ -179,6 +202,25 @@ static int lis2dw12_handle_double_tap_int(const struct device *dev)
 }
 #endif /* CONFIG_LIS2DW12_TAP */
 
+#ifdef CONFIG_LIS2DW12_THRESHOLD
+static int lis2dw12_handle_wu_ia_int(const struct device *dev)
+{
+	struct lis2dw12_data *lis2dw12 = dev->data;
+	sensor_trigger_handler_t handler = lis2dw12->threshold_handler;
+
+	struct sensor_trigger thresh_trig = {
+		.type = SENSOR_TRIG_THRESHOLD,
+		.chan = SENSOR_CHAN_ALL,
+	};
+
+	if (handler) {
+		handler(dev, &thresh_trig);
+	}
+
+	return 0;
+}
+#endif
+
 /**
  * lis2dw12_handle_interrupt - handle the drdy event
  * read data and call handler if registered any
@@ -202,6 +244,11 @@ static void lis2dw12_handle_interrupt(const struct device *dev)
 		lis2dw12_handle_double_tap_int(dev);
 	}
 #endif /* CONFIG_LIS2DW12_TAP */
+#ifdef CONFIG_LIS2DW12_THRESHOLD
+	if (sources.all_int_src.wu_ia) {
+		lis2dw12_handle_wu_ia_int(dev);
+	}
+#endif
 
 	gpio_pin_interrupt_configure_dt(&cfg->gpio_int,
 					GPIO_INT_EDGE_TO_ACTIVE);