From 876ab6c0157ac343e86957c3298a6b6179549a52 Mon Sep 17 00:00:00 2001 From: Mario Tesi Date: Tue, 24 Jan 2023 15:40:53 +0100 Subject: [PATCH] drivers:iio:stm:imu: add support to lsm6dsv16bx STMEMS IMU sensor Added support for the imu lsm6dsv16bx sensor. The current driver version supports the following features: - Acc and Gyro in continuous mode supported by the internal HW FIFO - QVAR sensor for presence and touch detection, data of QVAR are managed internally by HW FIFO. - SFLP Embedded Sensor Fusion Function Game Rotation vector sensor. - TDM slave interface configuration for accelerometer data. - MLC / FSM programmable functions. - Free fall, step counter, step detection, wake-up, significant motion, tilt, tap, double tap, 6D and sleep change embedded functions event detection. Signed-off-by: Mario Tesi Change-Id: I1f2ad688261cba90b5c3633ccd9fc6fb0301d771 --- .../bindings/iio/stm/imu/st_lsm6dsv16bx.txt | 95 + drivers/iio/stm/imu/Kconfig | 1 + drivers/iio/stm/imu/Makefile | 1 + drivers/iio/stm/imu/st_lsm6dsv16bx/Kconfig | 45 + drivers/iio/stm/imu/st_lsm6dsv16bx/Makefile | 11 + .../stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx.h | 991 +++++++++ .../st_lsm6dsv16bx/st_lsm6dsv16bx_buffer.c | 768 +++++++ .../imu/st_lsm6dsv16bx/st_lsm6dsv16bx_core.c | 1917 +++++++++++++++++ .../st_lsm6dsv16bx/st_lsm6dsv16bx_embfunc.c | 579 +++++ .../st_lsm6dsv16bx/st_lsm6dsv16bx_events.c | 1035 +++++++++ .../st_lsm6dsv16bx_hwtimestamp.c | 116 + .../imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i2c.c | 69 + .../imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i3c.c | 60 + .../imu/st_lsm6dsv16bx/st_lsm6dsv16bx_mlc.c | 847 ++++++++ .../st_lsm6dsv16bx_preload_mlc.h | 20 + .../imu/st_lsm6dsv16bx/st_lsm6dsv16bx_qvar.c | 191 ++ .../imu/st_lsm6dsv16bx/st_lsm6dsv16bx_spi.c | 67 + stm_iio_configs/lsm6dsv16bx_defconfig | 6 + 18 files changed, 6819 insertions(+) create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsv16bx.txt create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_embfunc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_events.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_hwtimestamp.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i3c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_mlc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_preload_mlc.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_qvar.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_spi.c create mode 100644 stm_iio_configs/lsm6dsv16bx_defconfig diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsv16bx.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsv16bx.txt new file mode 100644 index 000000000000..c5d1389eff83 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsv16bx.txt @@ -0,0 +1,95 @@ +* st_lsm6dsvx driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be "st,lsm6dsv16bx" + +Required properties for the i2c bindings: +- reg: i2c slave address + +Required properties for the spi bindings: +- reg: the chipselect index + +- spi-max-frequency: maximal bus speed, should be set to 1000000 unless + constrained by external circuitry + +Optional properties for all bus drivers: +- vdd-supply: an optional regulator that needs to be on to provide VDD + power to the sensor. + +- vddio-supply: an optional regulator that needs to be on to provide the + VDD IO power to the sensor. + +- st,int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- mount-matrix: mount rotation matrix. + + Refer to iio/mount-matrix.txt for details. + +- drive-open-drain: the interrupt/data ready line will be configured as open drain, + which is useful if several sensors share the same interrupt line. + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH, IRQ_TYPE_EDGE_RISING, + IRQ_TYPE_LEVEL_LOW or IRQ_TYPE_EDGE_FALLING. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +- enable-qvar: enable QVAR sensor feature. Default is disabled. + +- st,module_id: module identifier. + This is used by user-space to identify which devices + are part of the same module (particularly important for + supporting multiple sensors of the same type). + +- tdm_wclk: TDM word clock frequency selection: + - 0 8 kHz 2048 kHz (default) + - 1 16 kHz 2048 kHz + +- tdm_slot_sel: Selection of TDM slot for transmission. Default value: 0 + - 0: TDM slot {0, 1, 2} (default) + - 1: TDM slot {4, 5, 6} + +- tdm_ord_sel: Selects the order of transmission of the TDM axes + - 0 -> Z, Y, X + - 1 -> X, Z, Y + - 2 -> X, Y, Z (default) + +- tdm_fs_xl: TDM channel accelerometer full-scale selection: + - 0: ±2 g + - 1: ±4 g (default) + - 2: ±8 g + +Example for an spi device node: + +lsm6dsvx-imu@0 { + compatible = "st,lsm6dsv16x"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + vddio-supply = <&sensors_vddio>; + vdd-supply = <&sensors_vdd>; + st,int-pin = <1>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; + st,module_id = <2>; +}; + +Example for an i2c device node (SA0 connected to ground): + +lsm6dsvx-imu@6a { + compatible = "st,lsm6dsv16x"; + reg = <0x6a>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + vddio-supply = <&sensors_vddio>; + vdd-supply = <&sensors_vdd>; + st,int-pin = <1>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; + st,module_id = <1>; +}; diff --git a/drivers/iio/stm/imu/Kconfig b/drivers/iio/stm/imu/Kconfig index 50ec77095508..2dc9e51bae19 100644 --- a/drivers/iio/stm/imu/Kconfig +++ b/drivers/iio/stm/imu/Kconfig @@ -17,5 +17,6 @@ source "drivers/iio/stm/imu/st_lsm6ds3h/Kconfig" source "drivers/iio/stm/imu/st_lsm6dsm/Kconfig" source "drivers/iio/stm/imu/st_lsm6dsvx/Kconfig" source "drivers/iio/stm/imu/st_lsm6dso16is/Kconfig" +source "drivers/iio/stm/imu/st_lsm6dsv16bx/Kconfig" endmenu diff --git a/drivers/iio/stm/imu/Makefile b/drivers/iio/stm/imu/Makefile index 22d1f8729bfc..96d4383be9e6 100644 --- a/drivers/iio/stm/imu/Makefile +++ b/drivers/iio/stm/imu/Makefile @@ -15,3 +15,4 @@ obj-y += st_lsm6ds3h/ obj-y += st_lsm6dsm/ obj-y += st_lsm6dsvx/ obj-y += st_lsm6dso16is/ +obj-y += st_lsm6dsv16bx/ diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/Kconfig b/drivers/iio/stm/imu/st_lsm6dsv16bx/Kconfig new file mode 100644 index 000000000000..47895bfa9597 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/Kconfig @@ -0,0 +1,45 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM6DSV16BX + tristate "STMicroelectronics LSM6DSV16BX sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_LSM6DSV16BX_I2C if (I2C) + select IIO_ST_LSM6DSV16BX_SPI if (SPI_MASTER) + select IIO_ST_LSM6DSV16BX_I3C if (I3C) + help + Say yes here to build support for STMicroelectronics LSM6DSV16BX imu + sensor. + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsv16bx. + +config IIO_ST_LSM6DSV16BX_I2C + tristate + depends on IIO_ST_LSM6DSV16BX + +config IIO_ST_LSM6DSV16BX_SPI + tristate + depends on IIO_ST_LSM6DSV16BX + +config IIO_ST_LSM6DSV16BX_I3C + tristate + depends on IIO_ST_LSM6DSV16BX + select REGMAP_I3C + +config IIO_ST_LSM6DSV16BX_MLC_PRELOAD + bool "Preload some examples on MLC/FSM core" + depends on IIO_ST_LSM6DSV16BX + help + Select yes if you want to preload some examples on machine learning core + and finite state machine. + + The examples code is a motion intensity recognition and is hardcoded in the + driver in the mlcdata structure. + +config IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP + bool "Enable async hw timestamp read" + depends on IIO_ST_LSM6DSV16BX + help + Enable async task that sends over hw timestamp events. diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/Makefile b/drivers/iio/stm/imu/st_lsm6dsv16bx/Makefile new file mode 100644 index 000000000000..957c8a11b758 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/Makefile @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_lsm6dsv16bx-y := st_lsm6dsv16bx_core.o st_lsm6dsv16bx_buffer.o \ + st_lsm6dsv16bx_qvar.o st_lsm6dsv16bx_mlc.o \ + st_lsm6dsv16bx_events.o st_lsm6dsv16bx_embfunc.o + +st_lsm6dsv16bx-$(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) += st_lsm6dsv16bx_hwtimestamp.o + +obj-$(CONFIG_IIO_ST_LSM6DSV16BX) += st_lsm6dsv16bx.o +obj-$(CONFIG_IIO_ST_LSM6DSV16BX_I2C) += st_lsm6dsv16bx_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSV16BX_SPI) += st_lsm6dsv16bx_spi.o +obj-$(CONFIG_IIO_ST_LSM6DSV16BX_I3C) += st_lsm6dsv16bx_i3c.o diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx.h b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx.h new file mode 100644 index 000000000000..6e72887b943d --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx.h @@ -0,0 +1,991 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dsv16bx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSV16BX_H +#define ST_LSM6DSV16BX_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../../common/stm_iio_types.h" + +#define ST_LSM6DSV16BX_ODR_LIST_SIZE 9 +#define ST_LSM6DSV16BX_ODR_EXPAND(odr, uodr) ((odr * 1000000) + uodr) + +#define ST_LSM6DSV16BX_DEV_NAME "lsm6dsv16bx" + +#define ST_LSM6DSV16BX_SAMPLE_SIZE 6 +#define ST_LSM6DSV16BX_TS_SAMPLE_SIZE 4 +#define ST_LSM6DSV16BX_TAG_SIZE 1 +#define ST_LSM6DSV16BX_FIFO_SAMPLE_SIZE (ST_LSM6DSV16BX_SAMPLE_SIZE + \ + ST_LSM6DSV16BX_TAG_SIZE) +#define ST_LSM6DSV16BX_MAX_FIFO_DEPTH 208 + +/* register map */ +#define ST_LSM6DSV16BX_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_IF_CFG_ADDR 0x03 +#define ST_LSM6DSV16BX_PP_OD_MASK BIT(3) +#define ST_LSM6DSV16BX_H_LACTIVE_MASK BIT(4) +#define ST_LSM6DSV16BX_TDM_OUT_PU_EN_MASK BIT(6) + +#define ST_LSM6DSV16BX_REG_FIFO_CTRL1_ADDR 0x07 +#define ST_LSM6DSV16BX_WTM_MASK GENMASK(7, 0) + +#define ST_LSM6DSV16BX_REG_FIFO_CTRL3_ADDR 0x09 +#define ST_LSM6DSV16BX_BDR_XL_MASK GENMASK(3, 0) +#define ST_LSM6DSV16BX_BDR_GY_MASK GENMASK(7, 4) + +#define ST_LSM6DSV16BX_REG_FIFO_CTRL4_ADDR 0x0a +#define ST_LSM6DSV16BX_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_LSM6DSV16BX_ODR_T_BATCH_MASK GENMASK(5, 4) +#define ST_LSM6DSV16BX_DEC_TS_BATCH_MASK GENMASK(7, 6) + +#define ST_LSM6DSV16BX_COUNTER_BDR_REG1_ADDR 0x0b +#define ST_LSM6DSV16BX_AH_QVAR_BATCH_EN_MASK BIT(2) + +#define ST_LSM6DSV16BX_REG_INT1_CTRL_ADDR 0x0d +#define ST_LSM6DSV16BX_REG_INT2_CTRL_ADDR 0x0e +#define ST_LSM6DSV16BX_INT_FIFO_TH_MASK BIT(3) + +#define ST_LSM6DSV16BX_REG_WHOAMI_ADDR 0x0f +#define ST_LSM6DSV16BX_WHOAMI_VAL 0x71 + +#define ST_LSM6DSV16BX_REG_CTRL1_ADDR 0x10 +#define ST_LSM6DSV16BX_REG_CTRL2_ADDR 0x11 +#define ST_LSM6DSV16BX_ODR_MASK GENMASK(3, 0) +#define ST_LSM6DSV16BX_OP_MODE_MASK GENMASK(6, 4) + +#define ST_LSM6DSV16BX_REG_CTRL3_ADDR 0x12 +#define ST_LSM6DSV16BX_SW_RESET_MASK BIT(0) +#define ST_LSM6DSV16BX_BDU_MASK BIT(6) +#define ST_LSM6DSV16BX_BOOT_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_CTRL4_ADDR 0x13 +#define ST_LSM6DSV16BX_DRDY_MASK BIT(3) + +#define ST_LSM6DSV16BX_REG_CTRL6_ADDR 0x15 + +#define ST_LSM6DSV16BX_REG_CTRL7_ADDR 0x16 +#define ST_LSM6DSV16BX_AH_QVARx_EN_MASK GENMASK(3, 2) +#define ST_LSM6DSV16BX_AH_QVAR_C_ZIN_MASK GENMASK(5, 4) +#define ST_LSM6DSV16BX_AH_QVAR_EN_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_CTRL8_ADDR 0x17 + +#define ST_LSM6DSV16BX_REG_CTRL10_ADDR 0x19 +#define ST_LSM6DSV16BX_ST_XL_MASK GENMASK(1, 0) +#define ST_LSM6DSV16BX_ST_G_MASK GENMASK(3, 2) + +#define ST_LSM6DSV16BX_REG_FIFO_STATUS1_ADDR 0x1b +#define ST_LSM6DSV16BX_FIFO_DIFF_MASK GENMASK(8, 0) + +#define ST_LSM6DSV16BX_REG_ALL_INT_SRC_ADDR 0x1d +#define ST_LSM6DSV16BX_FF_IA_MASK BIT(0) +#define ST_LSM6DSV16BX_WU_IA_MASK BIT(1) +#define ST_LSM6DSV16BX_TAP_IA_MASK BIT(2) +#define ST_LSM6DSV16BX_D6D_IA_MASK BIT(4) +#define ST_LSM6DSV16BX_SLEEP_CHANGE_MASK BIT(5) + +#define ST_LSM6DSV16BX_REG_D6D_SRC_ADDR 0x1d +#define ST_LSM6DSV16BX_D6D_EVENT_MASK GENMASK(5, 0) + +#define ST_LSM6DSV16BX_REG_STATUS_REG_ADDR 0x1e +#define ST_LSM6DSV16BX_XLDA_MASK BIT(0) +#define ST_LSM6DSV16BX_GDA_MASK BIT(1) +#define ST_LSM6DSV16BX_TDA_MASK BIT(2) + +#define ST_LSM6DSV16BX_REG_OUT_TEMP_L_ADDR 0x20 + +#define ST_LSM6DSV16BX_REG_OUTZ_L_G_ADDR 0x22 +#define ST_LSM6DSV16BX_REG_OUTY_L_G_ADDR 0x24 +#define ST_LSM6DSV16BX_REG_OUTX_L_G_ADDR 0x26 +#define ST_LSM6DSV16BX_REG_OUTZ_L_A_ADDR 0x28 +#define ST_LSM6DSV16BX_REG_OUTY_L_A_ADDR 0x2a +#define ST_LSM6DSV16BX_REG_OUTX_L_A_ADDR 0x2c + +#define ST_LSM6DSV16BX_REG_OUT_QVAR_ADDR 0x3a + +#define ST_LSM6DSV16BX_REG_TIMESTAMP0_ADDR 0x40 +#define ST_LSM6DSV16BX_REG_TIMESTAMP2_ADDR 0x42 + +#define ST_LSM6DSV16BX_REG_WAKE_UP_SRC_ADDR 0x45 +#define ST_LSM6DSV16BX_WAKE_UP_EVENT_MASK GENMASK(3, 0) + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_STATUS_MAINPAGE_ADDR 0x49 +#define ST_LSM6DSV16BX_IS_STEP_DET_MASK BIT(3) +#define ST_LSM6DSV16BX_IS_TILT_MASK BIT(4) +#define ST_LSM6DSV16BX_IS_SIGMOT_MASK BIT(5) + +#define ST_LSM6DSV16BX_REG_FSM_STATUS_MAINPAGE_ADDR 0x4a +#define ST_LSM6DSV16BX_REG_MLC_STATUS_MAINPAGE_ADDR 0x4b + +#define ST_LSM6DSV16BX_REG_INTERNAL_FREQ_FINE 0x4f + +#define ST_LSM6DSV16BX_REG_FUNCTIONS_ENABLE_ADDR 0x50 +#define ST_LSM6DSV16BX_TIMESTAMP_EN_MASK BIT(6) +#define ST_LSM6DSV16BX_INTERRUPTS_ENABLE_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_TAP_CFG0_ADDR 0x56 +#define ST_LSM6DSV16BX_LIR_MASK BIT(0) +#define ST_LSM6DSV16BX_REG_TAP_Z_EN_MASK BIT(1) +#define ST_LSM6DSV16BX_REG_TAP_Y_EN_MASK BIT(2) +#define ST_LSM6DSV16BX_REG_TAP_X_EN_MASK BIT(3) +#define ST_LSM6DSV16BX_REG_TAP_EN_MASK GENMASK(3, 1) + +#define ST_LSM6DSV16BX_REG_TAP_CFG1_ADDR 0x57 +#define ST_LSM6DSV16BX_TAP_THS_X_MASK GENMASK(4, 0) +#define ST_LSM6DSV16BX_TAP_PRIORITY_MASK GENMASK(7, 5) + +#define ST_LSM6DSV16BX_REG_TAP_CFG2_ADDR 0x58 +#define ST_LSM6DSV16BX_TAP_THS_Y_MASK GENMASK(4, 0) + +#define ST_LSM6DSV16BX_REG_TAP_THS_6D_ADDR 0x59 +#define ST_LSM6DSV16BX_TAP_THS_Z_MASK GENMASK(4, 0) +#define ST_LSM6DSV16BX_SIXD_THS_MASK GENMASK(6, 5) + +#define ST_LSM6DSV16BX_REG_TAP_DUR_ADDR 0x5a +#define ST_LSM6DSV16BX_SHOCK_MASK GENMASK(1, 0) +#define ST_LSM6DSV16BX_QUIET_MASK GENMASK(3, 2) +#define ST_LSM6DSV16BX_DUR_MASK GENMASK(7, 4) + +#define ST_LSM6DSV16BX_REG_WAKE_UP_THS_ADDR 0x5b +#define ST_LSM6DSV16BX_WK_THS_MASK GENMASK(5, 0) +#define ST_LSM6DSV16BX_SINGLE_DOUBLE_TAP_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_WAKE_UP_DUR_ADDR 0x5c +#define ST_LSM6DSV16BX_WAKE_DUR_MASK GENMASK(6, 5) + +#define ST_LSM6DSV16BX_REG_FREE_FALL_ADDR 0x5d +#define ST_LSM6DSV16BX_FF_THS_MASK GENMASK(2, 0) + +#define ST_LSM6DSV16BX_REG_MD1_CFG_ADDR 0x5e +#define ST_LSM6DSV16BX_REG_MD2_CFG_ADDR 0x5f +#define ST_LSM6DSV16BX_REG_INT2_TIMESTAMP_MASK BIT(0) +#define ST_LSM6DSV16BX_REG_INT_EMB_FUNC_MASK BIT(1) +#define ST_LSM6DSV16BX_INT_6D_MASK BIT(2) +#define ST_LSM6DSV16BX_INT_DOUBLE_TAP_MASK BIT(3) +#define ST_LSM6DSV16BX_INT_FF_MASK BIT(4) +#define ST_LSM6DSV16BX_INT_WU_MASK BIT(5) +#define ST_LSM6DSV16BX_INT_SINGLE_TAP_MASK BIT(6) +#define ST_LSM6DSV16BX_INT_SLEEP_CHANGE_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_TDM_CFG0_ADDR 0x6c +#define ST_LSM6DSV16BX_REG_TDM_SLOT_SEL_MASK BIT(4) +#define ST_LSM6DSV16BX_REG_TDM_WCLK_MASK GENMASK(2, 1) +#define ST_LSM6DSV16BX_REG_TDM_WCLK_BCLK_SEL_MASK BIT(0) + +#define ST_LSM6DSV16BX_REG_TDM_CFG1_ADDR 0x6d +#define ST_LSM6DSV16BX_REG_TDM_AXES_ORD_SEL_MASK GENMASK(4, 3) + +#define ST_LSM6DSV16BX_REG_TDM_CFG2_ADDR 0x6e +#define ST_LSM6DSV16BX_REG_TDM_FS_XL_MASK GENMASK(1, 0) + +#define ST_LSM6DSV16BX_REG_FIFO_DATA_OUT_TAG_ADDR 0x78 + +/* embedded function registers */ +#define ST_LSM6DSV16BX_REG_PAGE_SEL_ADDR 0x02 +#define ST_LSM6DSV16BX_PAGE_SEL_MASK GENMASK(6, 4) + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_EN_A_ADDR 0x04 +#define ST_LSM6DSV16BX_SFLP_GAME_EN_MASK BIT(1) +#define ST_LSM6DSV16BX_REG_PEDO_EN_MASK BIT(3) +#define ST_LSM6DSV16BX_REG_TILT_EN_MASK BIT(4) +#define ST_LSM6DSV16BX_REG_SIGN_MOTION_EN_MASK BIT(5) + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_LSM6DSV16BX_FSM_EN_MASK BIT(0) +#define ST_LSM6DSV16BX_MLC_EN_MASK BIT(4) + +#define ST_LSM6DSV16BX_REG_PAGE_ADDRESS_ADDR 0x08 +#define ST_LSM6DSV16BX_REG_PAGE_VALUE_ADDR 0x09 + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_INT1_ADDR 0x0a +#define ST_LSM6DSV16BX_INT_STEP_DETECTOR_MASK BIT(3) +#define ST_LSM6DSV16BX_INT_TILT_MASK BIT(4) +#define ST_LSM6DSV16BX_INT_SIG_MOT_MASK BIT(5) + +#define ST_LSM6DSV16BX_REG_FSM_INT1_ADDR 0x0b +#define ST_LSM6DSV16BX_REG_MLC_INT1_ADDR 0x0d +#define ST_LSM6DSV16BX_REG_EMB_FUNC_INT2_ADDR 0x0e +#define ST_LSM6DSV16BX_REG_FSM_INT2_ADDR 0x0f +#define ST_LSM6DSV16BX_REG_MLC_INT2_ADDR 0x11 + +#define ST_LSM6DSV16BX_REG_FSM_STATUS_ADDR 0x13 +#define ST_LSM6DSV16BX_REG_MLC_STATUS_ADDR 0x15 + +#define ST_LSM6DSV16BX_REG_PAGE_RW_ADDR 0x17 +#define ST_LSM6DSV16BX_PAGE_READ_MASK BIT(5) +#define ST_LSM6DSV16BX_PAGE_WRITE_MASK BIT(6) +#define ST_LSM6DSV16BX_EMB_FUNC_LIR_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_FIFO_EN_A_ADDR 0x44 +#define ST_LSM6DSV16BX_SFLP_GAME_FIFO_EN BIT(1) +#define ST_LSM6DSV16BX_SFLP_GRAVITY_FIFO_EN BIT(4) +#define ST_LSM6DSV16BX_SFLP_GBIAS_FIFO_EN_MASK BIT(5) +#define ST_LSM6DSV16BX_STEP_COUNTER_FIFO_EN_MASK BIT(6) + +#define ST_LSM6DSV16BX_REG_FSM_ENABLE_ADDR 0x46 + +#define ST_LSM6DSV16BX_REG_FSM_OUTS1_ADDR 0x4c + +#define ST_LSM6DSV16BX_REG_SFLP_ODR_ADDR 0x5e +#define ST_LSM6DSV16BX_SFLP_GAME_ODR_MASK GENMASK(5, 3) + +#define ST_LSM6DSV16BX_REG_FSM_ODR_ADDR 0x5f +#define ST_LSM6DSV16BX_FSM_ODR_MASK GENMASK(5, 3) + +#define ST_LSM6DSV16BX_REG_MLC_ODR_ADDR 0x60 +#define ST_LSM6DSV16BX_MLC_ODR_MASK GENMASK(6, 4) + +#define ST_LSM6DSV16BX_REG_STEP_COUNTER_L_ADDR 0x62 + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_SRC_ADDR 0x64 +#define ST_LSM6DSV16BX_STEPCOUNTER_BIT_SET_MASK BIT(2) +#define ST_LSM6DSV16BX_STEP_OVERFLOW_MASK BIT(3) +#define ST_LSM6DSV16BX_STEP_COUNT_DELTA_IA_MASK BIT(4) +#define ST_LSM6DSV16BX_STEP_DETECTED_MASK BIT(5) +#define ST_LSM6DSV16BX_PEDO_RST_STEP_MASK BIT(7) + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_INIT_A_ADDR 0x66 +#define ST_LSM6DSV16BX_SFLP_GAME_INIT_MASK BIT(1) + +#define ST_LSM6DSV16BX_REG_EMB_FUNC_INIT_B_ADDR 0x67 +#define ST_LSM6DSV16BX_FSM_INIT_MASK BIT(0) +#define ST_LSM6DSV16BX_MLC_INIT_MASK BIT(4) + +#define ST_LSM6DSV16BX_REG_MLC1_SRC_ADDR 0x70 + +#define ST_LSM6DSV16BX_TS_DELTA_NS 21700ULL + +/* temperature in uC */ +#define ST_LSM6DSV16BX_TEMP_GAIN 256 +#define ST_LSM6DSV16BX_TEMP_OFFSET 6400 + +/* self test values */ +#define ST_LSM6DSV16BX_SELFTEST_ACCEL_MIN 410 +#define ST_LSM6DSV16BX_SELFTEST_ACCEL_MAX 13935 +#define ST_LSM6DSV16BX_SELFTEST_GYRO_MIN 2143 +#define ST_LSM6DSV16BX_SELFTEST_GYRO_MAX 10000 + +#define ST_LSM6DSV16BX_SELF_TEST_NORMAL_MODE_VAL 0 +#define ST_LSM6DSV16BX_SELF_TEST_POS_SIGN_VAL 1 +#define ST_LSM6DSV16BX_SELF_TEST_NEG_SIGN_VAL 2 + +#define ST_LSM6DSV16BX_DEFAULT_KTIME (200000000) +#define ST_LSM6DSV16BX_FAST_KTIME (5000000) + +#define ST_LSM6DSV16BX_DATA_CHANNEL(chan_type, addr, mod, ch2, scan_idx, \ + rb, sb, sg, ext_inf) \ +{ \ + .type = chan_type, \ + .address = addr, \ + .modified = mod, \ + .channel2 = ch2, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = sg, \ + .realbits = rb, \ + .storagebits = sb, \ + .endianness = IIO_LE, \ + }, \ + .ext_info = ext_inf, \ +} + +#define ST_LSM6DSV16BX_SFLP_DATA_CHANNEL(chan_type, mod, ch2, scan_idx, \ + rb, sb, sg) \ +{ \ + .type = chan_type, \ + .modified = mod, \ + .channel2 = ch2, \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = sg, \ + .realbits = rb, \ + .storagebits = sb, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_event_spec st_lsm6dsv16bx_flush_event = { + .type = STM_IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_lsm6dsv16bx_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_LSM6DSV16BX_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lsm6dsv16bx_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_LSM6DSV16BX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +struct st_lsm6dsv16bx_reg { + u8 addr; + u8 mask; +}; + +struct st_lsm6dsv16bx_odr { + u16 hz; + int uhz; + u8 val; + u8 batch_val; +}; + +struct st_lsm6dsv16bx_odr_table_entry { + u8 size; + struct st_lsm6dsv16bx_reg reg; + struct st_lsm6dsv16bx_odr odr_avl[ST_LSM6DSV16BX_ODR_LIST_SIZE]; +}; + +struct st_lsm6dsv16bx_fs { + u32 gain; + u8 val; +}; + +#define ST_LSM6DSV16BX_FS_LIST_SIZE 6 +#define ST_LSM6DSV16BX_FS_ACC_LIST_SIZE 4 +#define ST_LSM6DSV16BX_FS_GYRO_LIST_SIZE 6 +struct st_lsm6dsv16bx_fs_table_entry { + u8 size; + struct st_lsm6dsv16bx_reg reg; + struct st_lsm6dsv16bx_fs fs_avl[ST_LSM6DSV16BX_FS_LIST_SIZE]; +}; + +#define ST_LSM6DSV16BX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_LSM6DSV16BX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_LSM6DSV16BX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_LSM6DSV16BX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488000) + +#define ST_LSM6DSV16BX_GYRO_FS_125_GAIN IIO_DEGREE_TO_RAD(4375000) +#define ST_LSM6DSV16BX_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_LSM6DSV16BX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_LSM6DSV16BX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_LSM6DSV16BX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_LSM6DSV16BX_GYRO_FS_4000_GAIN IIO_DEGREE_TO_RAD(140000000) + +/** + * enum st_lsm6dsv16bx_hw_id - list of HW device id supported by the + * lsm6dsv16bx driver + */ +enum st_lsm6dsv16bx_hw_id { + ST_LSM6DSV16BX_ID, + ST_LSM6DSV16BX_MAX_ID, +}; + +enum st_lsm6dsv16bx_fsm_mlc_enable_id { + ST_LSM6DSV16BX_MLC_FSM_DISABLED = 0, + ST_LSM6DSV16BX_MLC_ENABLED = BIT(0), + ST_LSM6DSV16BX_FSM_ENABLED = BIT(1), +}; + +/** + * struct mlc_config_t - MLC/FSM configuration report struct + * @mlc_int_addr: interrupt register address. + * @mlc_int_mask: interrupt register mask. + * @fsm_int_addr: interrupt register address. + * @fsm_int_mask: interrupt register mask. + * @mlc_configured: number of mlc configured. + * @fsm_configured: number of fsm configured. + * @bin_len: fw binary size. + * @requested_odr: Min ODR requested to works properly. + * @status: MLC / FSM enabled status. + */ +struct st_lsm6dsv16bx_mlc_config_t { + uint8_t mlc_int_addr; + uint8_t mlc_int_mask; + uint8_t fsm_int_addr; + uint8_t fsm_int_mask; + uint8_t mlc_configured; + uint8_t fsm_configured; + uint16_t bin_len; + uint16_t requested_odr; + enum st_lsm6dsv16bx_fsm_mlc_enable_id status; +}; + +/** + * struct st_lsm6dsv16bx_settings - ST IMU sensor settings + * @hw_id: Hw id supported by the driver configuration. + * @name: Device name supported by the driver configuration. + * @st_qvar_probe: QVAR probe flag, indicate if QVAR feature is supported. + * @st_mlc_probe: MLC probe flag, indicate if MLC feature is supported. + * @st_fsm_probe: FSM probe flag, indicate if FSM feature is supported. + * @st_sflp_probe: SFLP probe flag, indicate if SFLP feature is supported. + * @fs_table: full scale table for main sensors. + * + * main sensors are ST_LSM6DSV16BX_ID_GYRO, ST_LSM6DSV16BX_ID_ACC and + * ST_LSM6DSV16BX_ID_TEMP + */ +#define ST_LSM6DSV16BX_MAIN_SENSOR_NUM 3 + +struct st_lsm6dsv16bx_settings { + struct { + enum st_lsm6dsv16bx_hw_id hw_id; + const char *name; + } id; + + bool st_qvar_probe; + bool st_mlc_probe; + bool st_fsm_probe; + bool st_sflp_probe; + bool st_tdm_probe; + struct st_lsm6dsv16bx_fs_table_entry fs_table[ST_LSM6DSV16BX_MAIN_SENSOR_NUM]; +}; + +enum st_lsm6dsv16bx_sensor_id { + ST_LSM6DSV16BX_ID_GYRO, + ST_LSM6DSV16BX_ID_ACC, + ST_LSM6DSV16BX_ID_TEMP, + ST_LSM6DSV16BX_ID_6X_GAME, + ST_LSM6DSV16BX_ID_QVAR, + ST_LSM6DSV16BX_ID_MLC, + ST_LSM6DSV16BX_ID_MLC_0, + ST_LSM6DSV16BX_ID_MLC_1, + ST_LSM6DSV16BX_ID_MLC_2, + ST_LSM6DSV16BX_ID_MLC_3, + ST_LSM6DSV16BX_ID_FSM_0, + ST_LSM6DSV16BX_ID_FSM_1, + ST_LSM6DSV16BX_ID_FSM_2, + ST_LSM6DSV16BX_ID_FSM_3, + ST_LSM6DSV16BX_ID_FSM_4, + ST_LSM6DSV16BX_ID_FSM_5, + ST_LSM6DSV16BX_ID_FSM_6, + ST_LSM6DSV16BX_ID_FSM_7, + ST_LSM6DSV16BX_ID_STEP_COUNTER, + ST_LSM6DSV16BX_ID_STEP_DETECTOR, + ST_LSM6DSV16BX_ID_SIGN_MOTION, + ST_LSM6DSV16BX_ID_TILT, + ST_LSM6DSV16BX_ID_TAP, + ST_LSM6DSV16BX_ID_DTAP, + ST_LSM6DSV16BX_ID_FF, + ST_LSM6DSV16BX_ID_SLPCHG, + ST_LSM6DSV16BX_ID_WK, + ST_LSM6DSV16BX_ID_6D, + ST_LSM6DSV16BX_ID_MAX, +}; + +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_main_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_GYRO, + [1] = ST_LSM6DSV16BX_ID_ACC, + [2] = ST_LSM6DSV16BX_ID_6X_GAME, + [3] = ST_LSM6DSV16BX_ID_TEMP, +}; + +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_gyro_dep_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_GYRO, + [1] = ST_LSM6DSV16BX_ID_6X_GAME, + [4] = ST_LSM6DSV16BX_ID_MLC, + [5] = ST_LSM6DSV16BX_ID_MLC_0, + [6] = ST_LSM6DSV16BX_ID_MLC_1, + [7] = ST_LSM6DSV16BX_ID_MLC_2, + [8] = ST_LSM6DSV16BX_ID_MLC_3, + [9] = ST_LSM6DSV16BX_ID_FSM_0, + [10] = ST_LSM6DSV16BX_ID_FSM_1, + [11] = ST_LSM6DSV16BX_ID_FSM_2, + [12] = ST_LSM6DSV16BX_ID_FSM_3, + [13] = ST_LSM6DSV16BX_ID_FSM_4, + [14] = ST_LSM6DSV16BX_ID_FSM_5, + [15] = ST_LSM6DSV16BX_ID_FSM_6, + [16] = ST_LSM6DSV16BX_ID_FSM_7, +}; + +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_acc_dep_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_ACC, + [1] = ST_LSM6DSV16BX_ID_TEMP, + [2] = ST_LSM6DSV16BX_ID_6X_GAME, + [3] = ST_LSM6DSV16BX_ID_QVAR, + [6] = ST_LSM6DSV16BX_ID_MLC, + [7] = ST_LSM6DSV16BX_ID_MLC_0, + [8] = ST_LSM6DSV16BX_ID_MLC_1, + [9] = ST_LSM6DSV16BX_ID_MLC_2, + [10] = ST_LSM6DSV16BX_ID_MLC_3, + [11] = ST_LSM6DSV16BX_ID_FSM_0, + [12] = ST_LSM6DSV16BX_ID_FSM_1, + [13] = ST_LSM6DSV16BX_ID_FSM_2, + [14] = ST_LSM6DSV16BX_ID_FSM_3, + [15] = ST_LSM6DSV16BX_ID_FSM_4, + [16] = ST_LSM6DSV16BX_ID_FSM_5, + [17] = ST_LSM6DSV16BX_ID_FSM_6, + [18] = ST_LSM6DSV16BX_ID_FSM_7, + [19] = ST_LSM6DSV16BX_ID_STEP_COUNTER, + [20] = ST_LSM6DSV16BX_ID_STEP_DETECTOR, + [21] = ST_LSM6DSV16BX_ID_SIGN_MOTION, + [22] = ST_LSM6DSV16BX_ID_TILT, + [23] = ST_LSM6DSV16BX_ID_TAP, + [24] = ST_LSM6DSV16BX_ID_DTAP, + [25] = ST_LSM6DSV16BX_ID_FF, + [26] = ST_LSM6DSV16BX_ID_SLPCHG, + [27] = ST_LSM6DSV16BX_ID_WK, + [28] = ST_LSM6DSV16BX_ID_6D, +}; + +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_buffered_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_GYRO, + [1] = ST_LSM6DSV16BX_ID_ACC, + [2] = ST_LSM6DSV16BX_ID_TEMP, + [3] = ST_LSM6DSV16BX_ID_6X_GAME, + [4] = ST_LSM6DSV16BX_ID_QVAR, + [7] = ST_LSM6DSV16BX_ID_STEP_COUNTER, +}; + +/** + * The mlc only sensor list used by mlc loader + */ +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_mlc_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_MLC_0, + [1] = ST_LSM6DSV16BX_ID_MLC_1, + [2] = ST_LSM6DSV16BX_ID_MLC_2, + [3] = ST_LSM6DSV16BX_ID_MLC_3, +}; + +/** + * The fsm only sensor list used by mlc loader + */ +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_fsm_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_FSM_0, + [1] = ST_LSM6DSV16BX_ID_FSM_1, + [2] = ST_LSM6DSV16BX_ID_FSM_2, + [3] = ST_LSM6DSV16BX_ID_FSM_3, + [4] = ST_LSM6DSV16BX_ID_FSM_4, + [5] = ST_LSM6DSV16BX_ID_FSM_5, + [6] = ST_LSM6DSV16BX_ID_FSM_6, + [7] = ST_LSM6DSV16BX_ID_FSM_7, +}; + +/** + * The low power embedded function only sensor list + */ +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_embfunc_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_STEP_COUNTER, + [1] = ST_LSM6DSV16BX_ID_STEP_DETECTOR, + [2] = ST_LSM6DSV16BX_ID_SIGN_MOTION, + [3] = ST_LSM6DSV16BX_ID_TILT, +}; + +/** + * The low power event only sensor list + */ +static const enum st_lsm6dsv16bx_sensor_id st_lsm6dsv16bx_event_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_TAP, + [1] = ST_LSM6DSV16BX_ID_DTAP, + [2] = ST_LSM6DSV16BX_ID_FF, + [3] = ST_LSM6DSV16BX_ID_SLPCHG, + [4] = ST_LSM6DSV16BX_ID_WK, + [5] = ST_LSM6DSV16BX_ID_6D, +}; + +/** + * The low power event triggered only sensor list + */ +static const enum st_lsm6dsv16bx_sensor_id +st_lsm6dsv16bx_event_trigger_sensor_list[] = { + [0] = ST_LSM6DSV16BX_ID_WK, + [1] = ST_LSM6DSV16BX_ID_6D, +}; + +#define ST_LSM6DSV16BX_ID_ALL_FSM_MLC (BIT(ST_LSM6DSV16BX_ID_MLC_0) | \ + BIT(ST_LSM6DSV16BX_ID_MLC_1) | \ + BIT(ST_LSM6DSV16BX_ID_MLC_2) | \ + BIT(ST_LSM6DSV16BX_ID_MLC_3) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_0) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_1) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_2) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_3) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_4) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_5) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_6) | \ + BIT(ST_LSM6DSV16BX_ID_FSM_7)) +enum st_lsm6dsv16bx_fifo_mode { + ST_LSM6DSV16BX_FIFO_BYPASS = 0x0, + ST_LSM6DSV16BX_FIFO_CONT = 0x6, +}; + +enum { + ST_LSM6DSV16BX_HW_FLUSH, + ST_LSM6DSV16BX_HW_OPERATIONAL, +}; + +/* sensor devices that can wake-up the target */ +#define ST_LSM6DSV16BX_WAKE_UP_SENSORS (BIT(ST_LSM6DSV16BX_ID_GYRO) | \ + BIT(ST_LSM6DSV16BX_ID_ACC) | \ + ST_LSM6DSV16BX_ID_ALL_FSM_MLC) + +/** + * struct st_lsm6dsv16bx_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dsv16bx_hw. + * @trig: Trigger used by IIO event sensors. + * @odr: Output data rate of the sensor [Hz]. + * @uodr: Output data rate of the sensor [uHz]. + * @gain: Configured sensor sensitivity. + * @offset: Sensor data offset. + * @std_samples: Counter of samples to discard during sensor bootstrap. + * @std_level: Samples to discard threshold. + * @decimator: Samples decimate counter. + * @dec_counter: Samples decimate value. + * @last_fifo_timestamp: Timestamp related to last sample in FIFO. + * @max_watermark: Max supported watermark level. + * @watermark: Sensor watermark level. + * @selftest_status: Report last self test status. + * @min_st: Min self test raw data value. + * @max_st: Max self test raw data value. + * @batch_reg: Batching register info (addr + mask). + * @status_reg: MLC/FSM IIO event sensor status register. + * @outreg_addr: MLC/FSM IIO event sensor output register. + * @status: MLC/FSM enabled IIO event sensor status. + * @conf: Used in case of IIO sensor event to store configuration. + * @scan: Scan buffer for triggered sensors event. + */ +struct st_lsm6dsv16bx_sensor { + char name[32]; + enum st_lsm6dsv16bx_sensor_id id; + struct st_lsm6dsv16bx_hw *hw; + struct iio_trigger *trig; + + int odr; + int uodr; + + union { + struct { + /* data sensors */ + u32 gain; + u32 offset; + + u8 std_samples; + u8 std_level; + + u8 decimator; + u8 dec_counter; + + s64 last_fifo_timestamp; + u16 max_watermark; + u16 watermark; + + /* self test */ + int8_t selftest_status; + int min_st; + int max_st; + + struct st_lsm6dsv16bx_reg batch_reg; + }; + + struct { + /* mlc or fsm sensor */ + uint8_t status_reg; + uint8_t outreg_addr; + enum st_lsm6dsv16bx_fsm_mlc_enable_id status; + }; + + struct { + /* event sensor, data configuration */ + u32 conf[6]; + + /* ensure natural alignment of timestamp */ + struct { + u8 event; + s64 ts __aligned(8); + } scan; + }; + }; +}; + +/** + * struct st_lsm6dsv16bx_hw - ST IMU MEMS hw instance + * @dev: Pointer to instance of device struct (I2C / SPI / I3C). + * @irq: Device interrupt line (I2C / SPI / I3C). + * @regmap: regmap structure pointer. + * @lock: Mutex to protect read and write operations. + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO. + * @page_lock: Mutex to prevent concurrent memory page configuration. + * @fifo_mode: FIFO operating mode supported by the device. + * @state: hw operational state. + * @enable_mask: Enabled sensor bitmask. + * @hw_timestamp_global: hw timestamp value always monotonic where the most + * significant 8byte are incremented at every disable/enable. + * @timesync_workqueue: runs the async task in private workqueue. + * @timesync_work: actual work to be done in the async task workqueue. + * @timesync_timer: hrtimer used to schedule period read for the async task. + * @hwtimestamp_lock: spinlock for the 64bit timestamp value. + * @timesync_ktime: interval value used by the hrtimer. + * @timestamp_c: counter used for counting number of timesync updates. + * @int_pin: selected interrupt pin from configuration. + * @ext_data_len: Number of i2c slave devices connected to I2C master. + * @ts_offset: Hw timestamp offset. + * @ts_delta_ns: Calibrated delta timestamp. + * @hw_ts: Latest hw timestamp from the sensor. + * @val_ts_old: Last sample timestamp for rollover check. + * @hw_ts_high: Manage timestamp rollover. + * @tsample: Sample timestamp. + * @delta_ts: Estimated delta time between two consecutive interrupts. + * @ts: Latest timestamp from irq handler. + * @module_id: identify iio devices of the same sensor module. + * @orientation: Sensor orientation matrix. + * @vdd_supply: Voltage regulator for VDD. + * @vddio_supply: Voltage regulator for VDDIIO. + * @mlc_config: MLC/FSM data register structure. + * @preload_mlc: MLC/FSM preload flag. + * @qvar_workqueue: QVAR workqueue (if enabled in Kconfig). + * @iio_devs: Pointers to acc/gyro iio_dev instances. + * @settings: ST IMU sensor settings. + * @fs_table: ST IMU full scale table. + * @odr_table: ST IMU output data rate table. + * @en_tdm: TDM enable flag. + */ +struct st_lsm6dsv16bx_hw { + struct device *dev; + int irq; + struct regmap *regmap; + struct mutex lock; + struct mutex fifo_lock; + struct mutex page_lock; + + enum st_lsm6dsv16bx_fifo_mode fifo_mode; + unsigned long state; + u32 enable_mask; + s64 hw_timestamp_global; + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + struct workqueue_struct *timesync_workqueue; + struct work_struct timesync_work; + struct hrtimer timesync_timer; + spinlock_t hwtimestamp_lock; + ktime_t timesync_ktime; + int timesync_c; +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + u8 int_pin; + + u8 ext_data_len; + + s64 ts_offset; + u64 ts_delta_ns; + s64 hw_ts; + u32 val_ts_old; + u32 hw_ts_high; + s64 tsample; + s64 delta_ts; + s64 ts; + + u32 module_id; + struct iio_mount_matrix orientation; + struct regulator *vdd_supply; + struct regulator *vddio_supply; + struct st_lsm6dsv16bx_mlc_config_t *mlc_config; + bool preload_mlc; + struct workqueue_struct *qvar_workqueue; + struct iio_dev *iio_devs[ST_LSM6DSV16BX_ID_MAX]; + + const struct st_lsm6dsv16bx_settings *settings; + const struct st_lsm6dsv16bx_fs_table_entry *fs_table; + const struct st_lsm6dsv16bx_odr_table_entry *odr_table; + + bool en_tdm; +}; + +/** + * struct st_lsm6dsv16bx_ff_th - Free Fall threshold table + * @mg: Threshold in mg. + * @val: Register value. + */ +struct st_lsm6dsv16bx_ff_th { + u32 mg; + u8 val; +}; + +/** + * struct st_lsm6dsv16bx_6D_th - 6D threshold table + * @deg: Threshold in degrees. + * @val: Register value. + */ +struct st_lsm6dsv16bx_6D_th { + u8 deg; + u8 val; +}; + +extern const struct dev_pm_ops st_lsm6dsv16bx_pm_ops; + +static inline int +__st_lsm6dsv16bx_write_with_mask(struct st_lsm6dsv16bx_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_LSM6DSV16BX_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int st_lsm6dsv16bx_write_with_mask(struct st_lsm6dsv16bx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dsv16bx_write_with_mask(hw, addr, mask, data); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsv16bx_read_locked(struct st_lsm6dsv16bx_hw *hw, unsigned int addr, + void *data, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, data, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsv16bx_write_locked(struct st_lsm6dsv16bx_hw *hw, unsigned int addr, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_write(hw->regmap, addr, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsv16bx_read_with_mask(struct st_lsm6dsv16bx_hw *hw, u8 addr, u8 mask, + u8 *val) +{ + u8 data; + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data)); + if (err < 0) + goto out; + + *val = (data & mask) >> __ffs(mask); + +out: + mutex_unlock(&hw->page_lock); + + return (err < 0) ? err : 0; +} +static inline int st_lsm6dsv16bx_set_page_access(struct st_lsm6dsv16bx_hw *hw, + unsigned int mask, + unsigned int data) +{ + return regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_FUNC_CFG_ACCESS_ADDR, + mask, + ST_LSM6DSV16BX_SHIFT_VAL(data, mask)); +} + +static inline bool st_lsm6dsv16bx_run_mlc_task(struct st_lsm6dsv16bx_hw *hw) +{ + return hw->settings->st_mlc_probe || hw->settings->st_fsm_probe; +} + +static inline bool +st_lsm6dsv16bx_is_fifo_enabled(struct st_lsm6dsv16bx_hw *hw) +{ + return hw->enable_mask & (BIT(ST_LSM6DSV16BX_ID_GYRO) | + BIT(ST_LSM6DSV16BX_ID_ACC) | + BIT(ST_LSM6DSV16BX_ID_QVAR)); +} + +int st_lsm6dsv16bx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap); +int st_lsm6dsv16bx_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable); +int st_lsm6dsv16bx_sflp_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable); +int st_lsm6dsv16bx_buffers_setup(struct st_lsm6dsv16bx_hw *hw); +ssize_t st_lsm6dsv16bx_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsv16bx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsv16bx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsv16bx_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsv16bx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); +int st_lsm6dsv16bx_suspend_fifo(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_set_fifo_mode(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_fifo_mode fifo_mode); +int +__st_lsm6dsv16bx_set_sensor_batching_odr(struct st_lsm6dsv16bx_sensor *sensor, + bool enable); +int st_lsm6dsv16bx_fsm_init(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_fsm_get_orientation(struct st_lsm6dsv16bx_hw *hw, u8 *data); +int st_lsm6dsv16bx_update_batching(struct iio_dev *iio_dev, bool enable); +int st_lsm6dsv16bx_get_batch_val(struct st_lsm6dsv16bx_sensor *sensor, + int odr, int uodr, u8 *val); + +int st_lsm6dsv16bx_qvar_probe(struct st_lsm6dsv16bx_hw *hw); +int +st_lsm6dsv16bx_qvar_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable); +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) +int st_lsm6dsv16bx_hwtimesync_init(struct st_lsm6dsv16bx_hw *hw); +#else /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ +static inline int +st_lsm6dsv16bx_hwtimesync_init(struct st_lsm6dsv16bx_hw *hw) +{ + return 0; +} +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + +int st_lsm6dsv16bx_mlc_probe(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_mlc_remove(struct device *dev); +int st_lsm6dsv16bx_mlc_check_status(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_mlc_init_preload(struct st_lsm6dsv16bx_hw *hw); + +int st_lsm6dsv16bx_probe_event(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_event_handler(struct st_lsm6dsv16bx_hw *hw); + +int st_lsm6dsv16bx_probe_embfunc(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_embfunc_handler_thread(struct st_lsm6dsv16bx_hw *hw); +int st_lsm6dsv16bx_step_counter_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable); +#endif /* ST_LSM6DSV16BX_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_buffer.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_buffer.c new file mode 100644 index 000000000000..601232820c57 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_buffer.c @@ -0,0 +1,768 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +#define ST_LSM6DSV16BX_SAMPLE_DISCHARD 0x7ffd +#define ST_LSM6DSV16BX_QVAR_FILTER_X 0x03AA +#define ST_LSM6DSV16BX_QVAR_SAMPLE_SIZE 2 + +enum { + ST_LSM6DSV16BX_GYRO_TAG = 0x01, + ST_LSM6DSV16BX_ACC_TAG = 0x02, + ST_LSM6DSV16BX_TEMP_TAG = 0x03, + ST_LSM6DSV16BX_TS_TAG = 0x04, + ST_LSM6DSV16BX_EXT0_TAG = 0x0f, + ST_LSM6DSV16BX_EXT1_TAG = 0x10, + ST_LSM6DSV16BX_STEPC_TAG = 0x12, + ST_LSM6DSV16BX_GAMEROT_TAG = 0x13, + ST_LSM6DSV16BX_QVAR_TAG = 0x1f, +}; + +#define ST_LSM6DSV16BX_EWMA_LEVEL 120 +#define ST_LSM6DSV16BX_EWMA_DIV 128 + +#define ST_LSM6DSV16BX_TIMESTAMP_RESET_VALUE 0xaa + +static inline s64 st_lsm6dsv16bx_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LSM6DSV16BX_EWMA_DIV - weight) * diff, + ST_LSM6DSV16BX_EWMA_DIV); + + return old + incr; +} + +static inline int st_lsm6dsv16bx_reset_hwts(struct st_lsm6dsv16bx_hw *hw) +{ + u8 data = ST_LSM6DSV16BX_TIMESTAMP_RESET_VALUE; + int ret; + + ret = st_lsm6dsv16bx_write_locked(hw, + ST_LSM6DSV16BX_REG_TIMESTAMP2_ADDR, + data); + if (ret < 0) + return ret; + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); + hw->hw_timestamp_global = (hw->hw_timestamp_global + (1LL << 32)) & + GENMASK_ULL(63, 32); + spin_unlock_irq(&hw->hwtimestamp_lock); + hw->timesync_c = 0; + hw->timesync_ktime = ktime_set(0, ST_LSM6DSV16BX_FAST_KTIME); +#else /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + hw->hw_timestamp_global = (hw->hw_timestamp_global + (1LL << 32)) & + GENMASK_ULL(63, 32); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + hw->ts = iio_get_time_ns(hw->iio_devs[0]); + hw->ts_offset = hw->ts; + hw->tsample = 0ull; + + return 0; +} + +int st_lsm6dsv16bx_set_fifo_mode(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_fifo_mode fifo_mode) +{ + int err; + + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSV16BX_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +int +__st_lsm6dsv16bx_set_sensor_batching_odr(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + u8 data = 0; + int err; + + if (enable) { + err = st_lsm6dsv16bx_get_batch_val(sensor, sensor->odr, + sensor->uodr, &data); + if (err < 0) + return err; + } + + if (sensor->id == ST_LSM6DSV16BX_ID_6X_GAME) { + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, 1); + err = __st_lsm6dsv16bx_write_with_mask(hw, + sensor->batch_reg.addr, + sensor->batch_reg.mask, + data); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, 0); + } else { + err = __st_lsm6dsv16bx_write_with_mask(hw, + sensor->batch_reg.addr, + sensor->batch_reg.mask, + data); + } + + return err; +} + +static inline int +st_lsm6dsv16bx_set_sensor_batching_odr(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dsv16bx_set_sensor_batching_odr(sensor, enable); + mutex_unlock(&hw->page_lock); + + return err; +} + +static int +st_lsm6dsv16bx_update_watermark(struct st_lsm6dsv16bx_sensor *sensor, + u16 watermark) +{ + u16 fifo_watermark = ST_LSM6DSV16BX_MAX_FIFO_DEPTH; + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + struct st_lsm6dsv16bx_sensor *cur_sensor; + u16 cur_watermark = 0; + __le16 wdata; + int data = 0; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_buffered_sensor_list); i++) { + enum st_lsm6dsv16bx_sensor_id id = st_lsm6dsv16bx_buffered_sensor_list[i]; + + if (!hw->iio_devs[id]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[id]); + + if (!(hw->enable_mask & BIT(cur_sensor->id))) + continue; + + cur_watermark = (cur_sensor == sensor) ? watermark + : cur_sensor->watermark; + + fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); + } + + fifo_watermark = max_t(u16, fifo_watermark, 2); + + mutex_lock(&hw->page_lock); + + err = regmap_read(hw->regmap, + ST_LSM6DSV16BX_REG_FIFO_CTRL1_ADDR + 1, &data); + if (err < 0) + goto out; + + fifo_watermark = ((data << 8) & ~ST_LSM6DSV16BX_WTM_MASK) | + (fifo_watermark & ST_LSM6DSV16BX_WTM_MASK); + wdata = cpu_to_le16(fifo_watermark); + err = regmap_bulk_write(hw->regmap, ST_LSM6DSV16BX_REG_FIFO_CTRL1_ADDR, + &wdata, sizeof(wdata)); +out: + mutex_unlock(&hw->page_lock); + + return err < 0 ? err : 0; +} + +static inline void st_lsm6dsv16bx_sync_hw_ts(struct st_lsm6dsv16bx_hw *hw, + s64 ts) +{ + s64 delta = ts -hw->hw_ts; + + hw->ts_offset = st_lsm6dsv16bx_ewma(hw->ts_offset, delta, + ST_LSM6DSV16BX_EWMA_LEVEL); +} + +static struct iio_dev * +st_lsm6dsv16bx_get_iiodev_from_tag(struct st_lsm6dsv16bx_hw *hw, u8 tag) +{ + struct iio_dev *iio_dev; + + switch (tag) { + case ST_LSM6DSV16BX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_GYRO]; + break; + case ST_LSM6DSV16BX_ACC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_ACC]; + break; + case ST_LSM6DSV16BX_TEMP_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_TEMP]; + break; + case ST_LSM6DSV16BX_STEPC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_STEP_COUNTER]; + break; + case ST_LSM6DSV16BX_QVAR_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_QVAR]; + break; + case ST_LSM6DSV16BX_GAMEROT_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_6X_GAME]; + break; + default: + iio_dev = NULL; + break; + } + + return iio_dev; +} + +static int st_lsm6dsv16bx_read_fifo(struct st_lsm6dsv16bx_hw *hw) +{ + u8 iio_buf[ALIGN(ST_LSM6DSV16BX_FIFO_SAMPLE_SIZE, sizeof(s64)) + + sizeof(s64) + sizeof(s64)]; + u8 buf[6 * ST_LSM6DSV16BX_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + struct st_lsm6dsv16bx_sensor *sensor; + __le64 hw_timestamp_push; + struct iio_dev *iio_dev; + s64 ts_irq, hw_ts_old; + __le16 fifo_status; + u16 fifo_depth; + s16 drdymask; + u32 val; + + ts_irq = hw->ts -hw->delta_ts; + + err = st_lsm6dsv16bx_read_locked(hw, + ST_LSM6DSV16BX_REG_FIFO_STATUS1_ADDR, + &fifo_status, sizeof(fifo_status)); + if (err < 0) + return err; + + fifo_depth = le16_to_cpu(fifo_status) & ST_LSM6DSV16BX_FIFO_DIFF_MASK; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_LSM6DSV16BX_FIFO_SAMPLE_SIZE; + read_len = 0; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_lsm6dsv16bx_read_locked(hw, + ST_LSM6DSV16BX_REG_FIFO_DATA_OUT_TAG_ADDR, + buf, word_len); + if (err < 0) + return err; + + for (i = 0; i < word_len; + i += ST_LSM6DSV16BX_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_LSM6DSV16BX_TAG_SIZE]; + tag = buf[i] >> 3; + + if (tag == ST_LSM6DSV16BX_TS_TAG) { + val = get_unaligned_le32(ptr); + if (hw->val_ts_old > val) + hw->hw_ts_high++; + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + hw->hw_timestamp_global = + (hw->hw_timestamp_global & + GENMASK_ULL(63, 32)) | + (u32)le32_to_cpu(val); + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + hw_ts_old = hw->hw_ts; + + /* check hw rollover */ + hw->val_ts_old = val; + hw->hw_ts = (val + + ((s64)hw->hw_ts_high << 32)) * + hw->ts_delta_ns; + hw->ts_offset = st_lsm6dsv16bx_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_LSM6DSV16BX_EWMA_LEVEL); + + if (!test_bit(ST_LSM6DSV16BX_HW_FLUSH, &hw->state)) + /* sync ap timestamp and sensor one */ + st_lsm6dsv16bx_sync_hw_ts(hw, ts_irq); + + ts_irq += hw->hw_ts; + + if (!hw->tsample) { + hw->tsample = hw->ts_offset + + hw->hw_ts; + } else { + hw->tsample = hw->tsample + + hw->hw_ts - + hw_ts_old; + } + } else { + iio_dev = st_lsm6dsv16bx_get_iiodev_from_tag(hw, tag); + if (!iio_dev) + continue; + + if (tag == ST_LSM6DSV16BX_QVAR_TAG) { + memcpy(iio_buf, ptr, + ST_LSM6DSV16BX_QVAR_SAMPLE_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + iio_get_time_ns(hw->iio_devs[0])); + } else { + drdymask = (s16)le16_to_cpu(get_unaligned_le16(ptr)); + if (unlikely(drdymask >= ST_LSM6DSV16BX_SAMPLE_DISCHARD)) + continue; + + sensor = iio_priv(iio_dev); + + /* + * hw ts in not queued in FIFO if only step + * counter enabled + */ + if (sensor->id == ST_LSM6DSV16BX_ID_STEP_COUNTER) { + val = get_unaligned_le32(ptr + 2); + hw->tsample = val * hw->ts_delta_ns; + } + + if ((tag == ST_LSM6DSV16BX_GYRO_TAG) || + (tag == ST_LSM6DSV16BX_ACC_TAG)) { + memcpy(&iio_buf[0], ptr + 4, 2); + memcpy(&iio_buf[2], ptr + 2, 2); + memcpy(&iio_buf[4], ptr, 2); + } else { + memcpy(iio_buf, ptr, + ST_LSM6DSV16BX_SAMPLE_SIZE); + } +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + hw_timestamp_push = cpu_to_le64(hw->hw_timestamp_global); + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + memcpy(&iio_buf[ALIGN(ST_LSM6DSV16BX_SAMPLE_SIZE, sizeof(s64))], + &hw_timestamp_push, sizeof(hw_timestamp_push)); + + /* avoid samples in the future */ + hw->tsample = min_t(s64, + iio_get_time_ns(hw->iio_devs[0]), + hw->tsample); + + /* support decimation for ODR < 15 Hz */ + if (sensor->dec_counter > 0) { + sensor->dec_counter--; + } else { + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + sensor->last_fifo_timestamp = hw_timestamp_push; + sensor->dec_counter = sensor->decimator; + } + } + } + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lsm6dsv16bx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +ssize_t st_lsm6dsv16bx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +ssize_t st_lsm6dsv16bx_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_update_watermark(sensor, val); + if (err < 0) + goto out; + + sensor->watermark = val; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +ssize_t st_lsm6dsv16bx_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + s64 type; + s64 event; + int count; + s64 ts; + s64 fifo_ts; + + mutex_lock(&hw->fifo_lock); + ts = iio_get_time_ns(iio_dev); + hw->delta_ts = ts -hw->ts; + hw->ts = ts; + set_bit(ST_LSM6DSV16BX_HW_FLUSH, &hw->state); + count = st_lsm6dsv16bx_read_fifo(hw); + sensor->dec_counter = 0; + fifo_ts = sensor->last_fifo_timestamp; + mutex_unlock(&hw->fifo_lock); + + type = count > 0 ? STM_IIO_EV_DIR_FIFO_DATA : STM_IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + STM_IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fifo_ts); + + return size; +} + +int st_lsm6dsv16bx_suspend_fifo(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + + st_lsm6dsv16bx_read_fifo(hw); + err = st_lsm6dsv16bx_set_fifo_mode(hw, ST_LSM6DSV16BX_FIFO_BYPASS); + + mutex_unlock(&hw->fifo_lock); + + return err; +} + +int st_lsm6dsv16bx_update_batching(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + err = st_lsm6dsv16bx_set_sensor_batching_odr(sensor, enable); + enable_irq(hw->irq); + + return err; +} + +int st_lsm6dsv16bx_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + hrtimer_cancel(&hw->timesync_timer); + cancel_work_sync(&hw->timesync_work); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + + switch (sensor->id) { + case ST_LSM6DSV16BX_ID_QVAR: + err = st_lsm6dsv16bx_qvar_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSV16BX_ID_6X_GAME: + err = st_lsm6dsv16bx_sflp_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSV16BX_ID_STEP_COUNTER: + err = st_lsm6dsv16bx_step_counter_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSV16BX_ID_TEMP: { + u8 data = 0; + /* + * this is an auxiliary sensor, it need to get batched + * toghether at least with a primary sensor (Acc/Gyro). + */ + if (!(hw->enable_mask & (BIT(ST_LSM6DSV16BX_ID_ACC) | + BIT(ST_LSM6DSV16BX_ID_GYRO)))) { + struct st_lsm6dsv16bx_sensor *acc_sensor; + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSV16BX_ID_ACC]); + if (enable) { + err = st_lsm6dsv16bx_get_batch_val(acc_sensor, + sensor->odr, + sensor->uodr, + &data); + if (err < 0) + goto out; + } + + /* batch main sensor */ + err = st_lsm6dsv16bx_write_with_mask(hw, + acc_sensor->batch_reg.addr, + acc_sensor->batch_reg.mask, + data); + if (err < 0) + goto out; + } + + if (enable) { + err = st_lsm6dsv16bx_get_batch_val(sensor, sensor->odr, + sensor->uodr, &data); + if (err < 0) + goto out; + } + + /* batch temperature sensor */ + err = st_lsm6dsv16bx_write_with_mask(hw, sensor->batch_reg.addr, + sensor->batch_reg.mask, + data); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + } + break; + default: + err = st_lsm6dsv16bx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_set_sensor_batching_odr(sensor, enable); + if (err < 0) + goto out; + + break; + } + + err = st_lsm6dsv16bx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (enable && hw->fifo_mode == ST_LSM6DSV16BX_FIFO_BYPASS) { + st_lsm6dsv16bx_reset_hwts(hw); + err = st_lsm6dsv16bx_set_fifo_mode(hw, ST_LSM6DSV16BX_FIFO_CONT); + } else if (!hw->enable_mask) { + err = st_lsm6dsv16bx_set_fifo_mode(hw, ST_LSM6DSV16BX_FIFO_BYPASS); + } + +#if defined(CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP) + if (hw->fifo_mode != ST_LSM6DSV16BX_FIFO_BYPASS) { + hrtimer_start(&hw->timesync_timer, + ktime_set(0, 0), + HRTIMER_MODE_REL); + } +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP */ + +out: + enable_irq(hw->irq); + + return err; +} + +static irqreturn_t st_lsm6dsv16bx_handler_irq(int irq, void *private) +{ + struct st_lsm6dsv16bx_hw *hw = (struct st_lsm6dsv16bx_hw *)private; + s64 ts = iio_get_time_ns(hw->iio_devs[0]); + + hw->delta_ts = ts -hw->ts; + hw->ts = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lsm6dsv16bx_handler_thread(int irq, void *private) +{ + struct st_lsm6dsv16bx_hw *hw = (struct st_lsm6dsv16bx_hw *)private; + + if (st_lsm6dsv16bx_run_mlc_task(hw)) + st_lsm6dsv16bx_mlc_check_status(hw); + + mutex_lock(&hw->fifo_lock); + st_lsm6dsv16bx_read_fifo(hw); + clear_bit(ST_LSM6DSV16BX_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + + st_lsm6dsv16bx_event_handler(hw); + st_lsm6dsv16bx_embfunc_handler_thread(hw); + + return IRQ_HANDLED; +} + +static int st_lsm6dsv16bx_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsv16bx_update_fifo(iio_dev, true); +} + +static int st_lsm6dsv16bx_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsv16bx_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsv16bx_fifo_ops = { + .preenable = st_lsm6dsv16bx_fifo_preenable, + .postdisable = st_lsm6dsv16bx_fifo_postdisable, +}; + +static int st_lsm6dsv16bx_fifo_init(struct st_lsm6dsv16bx_hw *hw) +{ + return st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSV16BX_DEC_TS_BATCH_MASK, 1); +} + +static const struct iio_trigger_ops st_lsm6dsv16bx_trigger_ops = { + NULL +}; + +int st_lsm6dsv16bx_buffers_setup(struct st_lsm6dsv16bx_hw *hw) +{ + struct device_node *np = hw->dev->of_node; +#if KERNEL_VERSION(5, 13, 0) > LINUX_VERSION_CODE + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + unsigned long irq_type; + bool irq_active_low; + int i, err; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); + if (irq_type == IRQF_TRIGGER_NONE) + irq_type = IRQF_TRIGGER_HIGH; + + switch (irq_type) { + case IRQF_TRIGGER_HIGH: + case IRQF_TRIGGER_RISING: + irq_active_low = false; + break; + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_FALLING: + irq_active_low = true; + break; + default: + dev_info(hw->dev, "mode %lx unsupported\n", irq_type); + return -EINVAL; + } + + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_IF_CFG_ADDR, + ST_LSM6DSV16BX_H_LACTIVE_MASK, + irq_active_low); + if (err < 0) + return err; + + if (np && of_property_read_bool(np, "drive-open-drain")) { + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_IF_CFG_ADDR, + ST_LSM6DSV16BX_PP_OD_MASK, 1); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lsm6dsv16bx_handler_irq, + st_lsm6dsv16bx_handler_thread, + irq_type | IRQF_ONESHOT, + hw->settings->id.name, hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return err; + } + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_buffered_sensor_list); i++) { + enum st_lsm6dsv16bx_sensor_id id = + st_lsm6dsv16bx_buffered_sensor_list[i]; + + if (!hw->iio_devs[id]) + continue; + +#if KERNEL_VERSION(5, 19, 0) <= LINUX_VERSION_CODE + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[id], + &st_lsm6dsv16bx_fifo_ops); + if (err) + return err; +#elif KERNEL_VERSION(5, 13, 0) <= LINUX_VERSION_CODE + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[id], + INDIO_BUFFER_SOFTWARE, + &st_lsm6dsv16bx_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + /* check if already allocated (maybe qvar) */ + if (!hw->iio_devs[id]->buffer) { + iio_device_attach_buffer(hw->iio_devs[id], buffer); + hw->iio_devs[id]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[id]->setup_ops = &st_lsm6dsv16bx_fifo_ops; + } +#endif /* LINUX_VERSION_CODE */ + + } + + err = st_lsm6dsv16bx_hwtimesync_init(hw); + if (err) + return err; + + return st_lsm6dsv16bx_fifo_init(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_core.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_core.c new file mode 100644 index 000000000000..d436c1c5ca11 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_core.c @@ -0,0 +1,1917 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx imu sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsv16bx.h" + +/** + * List of supported self test mode + */ +static struct st_lsm6dsv16bx_selftest_table_t { + char *smode; + u8 value; + u8 mask; +} st_lsm6dsv16bx_selftest_table[] = { + [0] = { + .smode = "disabled", + .value = ST_LSM6DSV16BX_SELF_TEST_NORMAL_MODE_VAL, + }, + [1] = { + .smode = "positive-sign", + .value = ST_LSM6DSV16BX_SELF_TEST_POS_SIGN_VAL, + }, + [2] = { + .smode = "negative-sign", + .value = ST_LSM6DSV16BX_SELF_TEST_NEG_SIGN_VAL, + }, +}; + +/** + * List of supported device settings + * + * The following table list all device supported by st_lsm6dsv16bx driver. + */ +static const struct st_lsm6dsv16bx_settings st_lsm6dsv16bx_sensor_settings[] = { + { + .id = { + .hw_id = ST_LSM6DSV16BX_ID, + .name = ST_LSM6DSV16BX_DEV_NAME, + }, + .st_qvar_probe = true, + .st_mlc_probe = true, + .st_fsm_probe = true, + .st_sflp_probe = true, + .st_tdm_probe = true, + .fs_table = { + [ST_LSM6DSV16BX_ID_ACC] = { + .size = 4, + .reg = { + .addr = ST_LSM6DSV16BX_REG_CTRL8_ADDR, + .mask = GENMASK(1, 0), + }, + .fs_avl[0] = { ST_LSM6DSV16BX_ACC_FS_2G_GAIN, 0x0 }, + .fs_avl[1] = { ST_LSM6DSV16BX_ACC_FS_4G_GAIN, 0x1 }, + .fs_avl[2] = { ST_LSM6DSV16BX_ACC_FS_8G_GAIN, 0x2 }, + .fs_avl[3] = { ST_LSM6DSV16BX_ACC_FS_16G_GAIN, 0x3 }, + }, + [ST_LSM6DSV16BX_ID_GYRO] = { + .size = 6, + .reg = { + .addr = ST_LSM6DSV16BX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .fs_avl[0] = { ST_LSM6DSV16BX_GYRO_FS_125_GAIN, 0x0 }, + .fs_avl[1] = { ST_LSM6DSV16BX_GYRO_FS_250_GAIN, 0x1 }, + .fs_avl[2] = { ST_LSM6DSV16BX_GYRO_FS_500_GAIN, 0x2 }, + .fs_avl[3] = { ST_LSM6DSV16BX_GYRO_FS_1000_GAIN, 0x3 }, + .fs_avl[4] = { ST_LSM6DSV16BX_GYRO_FS_2000_GAIN, 0x4 }, + .fs_avl[5] = { ST_LSM6DSV16BX_GYRO_FS_4000_GAIN, 0x6 }, + }, + [ST_LSM6DSV16BX_ID_TEMP] = { + .size = 1, + .fs_avl[0] = { (1000000 / ST_LSM6DSV16BX_TEMP_GAIN), 0x0 }, + }, + }, + }, +}; + +static const struct st_lsm6dsv16bx_odr_table_entry +st_lsm6dsv16bx_odr_table[] = { + [ST_LSM6DSV16BX_ID_ACC] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSV16BX_REG_CTRL1_ADDR, + .mask = ST_LSM6DSV16BX_ODR_MASK, + }, + /* odr val batch */ + .odr_avl[0] = { 7, 500000, 0x02, 0x02 }, + .odr_avl[1] = { 15, 0, 0x03, 0x03 }, + .odr_avl[2] = { 30, 0, 0x04, 0x04 }, + .odr_avl[3] = { 60, 0, 0x05, 0x05 }, + .odr_avl[4] = { 120, 0, 0x06, 0x06 }, + .odr_avl[5] = { 240, 0, 0x07, 0x07 }, + .odr_avl[6] = { 480, 0, 0x08, 0x08 }, + .odr_avl[7] = { 960, 0, 0x09, 0x09 }, + }, + [ST_LSM6DSV16BX_ID_GYRO] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSV16BX_REG_CTRL2_ADDR, + .mask = ST_LSM6DSV16BX_ODR_MASK, + }, + /* G LP MODE 7 Hz batch 7 Hz */ + .odr_avl[0] = { 7, 500000, 0x02, 0x02 }, + .odr_avl[1] = { 15, 0, 0x03, 0x03 }, + .odr_avl[2] = { 30, 0, 0x04, 0x04 }, + .odr_avl[3] = { 60, 0, 0x05, 0x05 }, + .odr_avl[4] = { 120, 0, 0x06, 0x06 }, + .odr_avl[5] = { 240, 0, 0x07, 0x07 }, + .odr_avl[6] = { 480, 0, 0x08, 0x08 }, + .odr_avl[7] = { 960, 0, 0x09, 0x09 }, + }, + [ST_LSM6DSV16BX_ID_TEMP] = { + .size = 3, + .odr_avl[0] = { 1, 875000, 0x00, 0x01 }, + .odr_avl[1] = { 15, 0, 0x00, 0x02 }, + .odr_avl[2] = { 60, 0, 0x00, 0x03 }, + }, + [ST_LSM6DSV16BX_ID_6X_GAME] = { + .size = 6, + .odr_avl[0] = { 15, 0, 0x00, 0x00 }, + .odr_avl[1] = { 30, 0, 0x00, 0x01 }, + .odr_avl[2] = { 60, 0, 0x00, 0x02 }, + .odr_avl[3] = { 120, 0, 0x00, 0x03 }, + .odr_avl[4] = { 240, 0, 0x00, 0x04 }, + .odr_avl[5] = { 480, 0, 0x00, 0x05 }, + }, +}; + +static const struct iio_mount_matrix * +st_lsm6dsv16bx_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *ch) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct iio_chan_spec_ext_info st_lsm6dsv16bx_chan_spec_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, st_lsm6dsv16bx_get_mount_matrix), + { } +}; + +#define IIO_CHAN_HW_TIMESTAMP(si) { \ + .type = IIO_COUNT, \ + .address = ST_LSM6DSV16BX_REG_TIMESTAMP0_ADDR, \ + .scan_index = si, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 64, \ + .storagebits = 64, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_lsm6dsv16bx_acc_channels[] = { + ST_LSM6DSV16BX_DATA_CHANNEL(IIO_ACCEL, + ST_LSM6DSV16BX_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsv16bx_chan_spec_ext_info), + ST_LSM6DSV16BX_DATA_CHANNEL(IIO_ACCEL, + ST_LSM6DSV16BX_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsv16bx_chan_spec_ext_info), + ST_LSM6DSV16BX_DATA_CHANNEL(IIO_ACCEL, + ST_LSM6DSV16BX_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsv16bx_chan_spec_ext_info), + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_gyro_channels[] = { + ST_LSM6DSV16BX_DATA_CHANNEL(IIO_ANGL_VEL, + ST_LSM6DSV16BX_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsv16bx_chan_spec_ext_info), + ST_LSM6DSV16BX_DATA_CHANNEL(IIO_ANGL_VEL, + ST_LSM6DSV16BX_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsv16bx_chan_spec_ext_info), + ST_LSM6DSV16BX_DATA_CHANNEL(IIO_ANGL_VEL, + ST_LSM6DSV16BX_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsv16bx_chan_spec_ext_info), + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LSM6DSV16BX_REG_OUT_TEMP_L_ADDR, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_OFFSET) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + } + }, + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_HW_TIMESTAMP(1), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_sflp_channels[] = { + ST_LSM6DSV16BX_SFLP_DATA_CHANNEL(IIO_ROT, 1, IIO_MOD_X, + 0, 16, 16, 'u'), + ST_LSM6DSV16BX_SFLP_DATA_CHANNEL(IIO_ROT, 1, IIO_MOD_Y, + 1, 16, 16, 'u'), + ST_LSM6DSV16BX_SFLP_DATA_CHANNEL(IIO_ROT, 1, IIO_MOD_Z, + 2, 16, 16, 'u'), + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static int st_lsm6dsv16bx_check_whoami(struct st_lsm6dsv16bx_hw *hw, int id) +{ + int data, err, i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_sensor_settings); i++) { + if (st_lsm6dsv16bx_sensor_settings[i].id.name && + st_lsm6dsv16bx_sensor_settings[i].id.hw_id == id) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsv16bx_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_LSM6DSV16BX_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + + return err; + } + + if (data != ST_LSM6DSV16BX_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + + return -ENODEV; + } + + hw->settings = &st_lsm6dsv16bx_sensor_settings[i]; + hw->fs_table = hw->settings->fs_table; + hw->odr_table = st_lsm6dsv16bx_odr_table; + + return 0; +} + +static int st_lsm6dsv16bx_get_odr_calibration(struct st_lsm6dsv16bx_hw *hw) +{ + s64 odr_calib; + int data; + int err; + + err = regmap_read(hw->regmap, + ST_LSM6DSV16BX_REG_INTERNAL_FREQ_FINE, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %d register\n", + ST_LSM6DSV16BX_REG_INTERNAL_FREQ_FINE); + + return err; + } + + odr_calib = (data * 37500) / 1000; + hw->ts_delta_ns = ST_LSM6DSV16BX_TS_DELTA_NS - odr_calib; + + dev_info(hw->dev, "Freq Fine %lld (ts %lld)\n", + odr_calib, hw->ts_delta_ns); + + return 0; +} + +static int +st_lsm6dsv16bx_set_full_scale(struct st_lsm6dsv16bx_sensor *sensor, u32 gain) +{ + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int i, err; + u8 val; + + for (i = 0; i < hw->fs_table[id].size; i++) + if (hw->fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == hw->fs_table[id].size) + return -EINVAL; + + val = hw->fs_table[id].fs_avl[i].val; + err = st_lsm6dsv16bx_write_with_mask(sensor->hw, + hw->fs_table[id].reg.addr, + hw->fs_table[id].reg.mask, + val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dsv16bx_get_odr_val(enum st_lsm6dsv16bx_sensor_id id, + int odr, int uodr, int *podr, + int *puodr, u8 *val) +{ + int required_odr = ST_LSM6DSV16BX_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < st_lsm6dsv16bx_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSV16BX_ODR_EXPAND( + st_lsm6dsv16bx_odr_table[id].odr_avl[i].hz, + st_lsm6dsv16bx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= required_odr) + break; + } + + if (i == st_lsm6dsv16bx_odr_table[id].size) + return -EINVAL; + + *val = st_lsm6dsv16bx_odr_table[id].odr_avl[i].val; + + if (podr && puodr) { + *podr = st_lsm6dsv16bx_odr_table[id].odr_avl[i].hz; + *puodr = st_lsm6dsv16bx_odr_table[id].odr_avl[i].uhz; + } + + return 0; +} + +int st_lsm6dsv16bx_get_batch_val(struct st_lsm6dsv16bx_sensor *sensor, + int odr, int uodr, u8 *val) +{ + int required_odr = ST_LSM6DSV16BX_ODR_EXPAND(odr, uodr); + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + int sensor_odr; + int i; + + for (i = 0; i < st_lsm6dsv16bx_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSV16BX_ODR_EXPAND( + st_lsm6dsv16bx_odr_table[id].odr_avl[i].hz, + st_lsm6dsv16bx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= required_odr) + break; + } + + if (i == st_lsm6dsv16bx_odr_table[id].size) + return -EINVAL; + + *val = st_lsm6dsv16bx_odr_table[id].odr_avl[i].batch_val; + + return 0; +} + +static int +st_lsm6dsv16bx_set_hw_sensor_odr(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id, + int req_odr, int req_uodr) +{ + int err; + u8 val = 0; + + if (ST_LSM6DSV16BX_ODR_EXPAND(req_odr, req_uodr) > 0) { + err = st_lsm6dsv16bx_get_odr_val(id, req_odr, req_uodr, + &req_odr, &req_uodr, &val); + if (err < 0) + return err; + } + + err = st_lsm6dsv16bx_write_with_mask(hw, + st_lsm6dsv16bx_odr_table[id].reg.addr, + st_lsm6dsv16bx_odr_table[id].reg.mask, + val); + + return err < 0 ? err : 0; +} + +static u16 +st_lsm6dsv16bx_check_odr_dependency(struct st_lsm6dsv16bx_hw *hw, int odr, + int uodr, + enum st_lsm6dsv16bx_sensor_id ref_id) +{ + struct st_lsm6dsv16bx_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = ST_LSM6DSV16BX_ODR_EXPAND(odr, uodr) > 0; + u16 ret; + + if (enable) { + /* uodr not used */ + if (hw->enable_mask & BIT(ref_id)) + ret = max_t(int, ref->odr, odr); + else + ret = odr; + } else { + ret = (hw->enable_mask & BIT(ref_id)) ? ref->odr : 0; + } + + return ret; +} + +static int +st_lsm6dsv16bx_check_acc_odr_dependency(struct st_lsm6dsv16bx_sensor *sensor, + int req_odr, int req_uodr) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + enum st_lsm6dsv16bx_sensor_id id; + int odr = 0; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_acc_dep_sensor_list); i++) { + id = st_lsm6dsv16bx_acc_dep_sensor_list[i]; + if (!hw->iio_devs[id]) + continue; + + if (id == sensor->id) + continue; + + /* req_uodr not used */ + odr = st_lsm6dsv16bx_check_odr_dependency(hw, req_odr, + req_uodr, id); + if (odr != req_odr) + return 0; + } + + return odr; +} + +static int +st_lsm6dsv16bx_check_gyro_odr_dependency(struct st_lsm6dsv16bx_sensor *sensor, + int req_odr, int req_uodr) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + enum st_lsm6dsv16bx_sensor_id id; + int odr = 0; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_gyro_dep_sensor_list); i++) { + id = st_lsm6dsv16bx_gyro_dep_sensor_list[i]; + if (!hw->iio_devs[id]) + continue; + + if (id == sensor->id) + continue; + + /* req_uodr not used */ + odr = st_lsm6dsv16bx_check_odr_dependency(hw, req_odr, + req_uodr, id); + if (odr != req_odr) + return 0; + } + + return odr; +} +static int st_lsm6dsv16bx_set_odr(struct st_lsm6dsv16bx_sensor *sensor, + int req_odr, int req_uodr) +{ + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err, odr; + + switch (id) { + case ST_LSM6DSV16BX_ID_QVAR: + case ST_LSM6DSV16BX_ID_FSM_0: + case ST_LSM6DSV16BX_ID_FSM_1: + case ST_LSM6DSV16BX_ID_FSM_2: + case ST_LSM6DSV16BX_ID_FSM_3: + case ST_LSM6DSV16BX_ID_FSM_4: + case ST_LSM6DSV16BX_ID_FSM_5: + case ST_LSM6DSV16BX_ID_FSM_6: + case ST_LSM6DSV16BX_ID_FSM_7: + case ST_LSM6DSV16BX_ID_MLC_0: + case ST_LSM6DSV16BX_ID_MLC_1: + case ST_LSM6DSV16BX_ID_MLC_2: + case ST_LSM6DSV16BX_ID_MLC_3: + case ST_LSM6DSV16BX_ID_TEMP: + case ST_LSM6DSV16BX_ID_STEP_COUNTER: + case ST_LSM6DSV16BX_ID_STEP_DETECTOR: + case ST_LSM6DSV16BX_ID_SIGN_MOTION: + case ST_LSM6DSV16BX_ID_TILT: + case ST_LSM6DSV16BX_ID_TAP: + case ST_LSM6DSV16BX_ID_DTAP: + case ST_LSM6DSV16BX_ID_WK: + case ST_LSM6DSV16BX_ID_FF: + case ST_LSM6DSV16BX_ID_SLPCHG: + case ST_LSM6DSV16BX_ID_6D: + case ST_LSM6DSV16BX_ID_ACC: + odr = st_lsm6dsv16bx_check_acc_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + return st_lsm6dsv16bx_set_hw_sensor_odr(hw, ST_LSM6DSV16BX_ID_ACC, + req_odr, req_uodr); + case ST_LSM6DSV16BX_ID_6X_GAME: + odr = st_lsm6dsv16bx_check_acc_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + err = st_lsm6dsv16bx_set_hw_sensor_odr(hw, ST_LSM6DSV16BX_ID_ACC, + req_odr, req_uodr); + if (err < 0) + return err; + + odr = st_lsm6dsv16bx_check_gyro_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + return st_lsm6dsv16bx_set_hw_sensor_odr(hw, ST_LSM6DSV16BX_ID_GYRO, + req_odr, req_uodr); + case ST_LSM6DSV16BX_ID_GYRO: + odr = st_lsm6dsv16bx_check_gyro_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + return st_lsm6dsv16bx_set_hw_sensor_odr(hw, ST_LSM6DSV16BX_ID_GYRO, + req_odr, req_uodr); + default: + break; + } + + return 0; +} + +int +st_lsm6dsv16bx_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsv16bx_set_odr(sensor, odr, uodr); + if (err < 0) + return err; + + if (enable) + sensor->hw->enable_mask |= BIT(sensor->id); + else + sensor->hw->enable_mask &= ~BIT(sensor->id); + + return 0; +} + +int st_lsm6dsv16bx_sflp_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err; + + //if (sensor->id != ST_LSM6DSV16BX_ID_6X_GAME) + // return -EINVAL; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_INIT_A_ADDR, + ST_LSM6DSV16BX_SFLP_GAME_INIT_MASK, + enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_EN_A_ADDR, + ST_LSM6DSV16BX_SFLP_GAME_EN_MASK, + enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_FIFO_EN_A_ADDR, + ST_LSM6DSV16BX_SFLP_GAME_FIFO_EN, + enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsv16bx_set_sensor_batching_odr(sensor, enable); + +reset_page: + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); +unlock: + mutex_unlock(&hw->page_lock); + + return st_lsm6dsv16bx_sensor_set_enable(sensor, enable); +} + +static int st_lsm6dsv16bx_enable_tdm(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + return st_lsm6dsv16bx_write_with_mask(sensor->hw, + ST_LSM6DSV16BX_REG_CTRL1_ADDR, + ST_LSM6DSV16BX_OP_MODE_MASK, + enable ? 0x02 : 0); +} + +static int +st_lsm6dsv16bx_read_oneshot(struct st_lsm6dsv16bx_sensor *sensor, u8 addr, + int *val) +{ + int err, delay; + __le16 data; + + err = st_lsm6dsv16bx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsv16bx_read_locked(sensor->hw, addr, + (u8 *)&data, sizeof(data)); + if (err < 0) + return err; + + st_lsm6dsv16bx_sensor_set_enable(sensor, false); + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dsv16bx_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + switch (mask) { + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + case IIO_ACCEL: + return IIO_VAL_INT_PLUS_NANO; + case IIO_TEMP: + return IIO_VAL_FRACTIONAL; + default: + return IIO_VAL_INT_PLUS_MICRO; + } + default: + return IIO_VAL_INT_PLUS_MICRO; + } + + return -EINVAL; +} + +static int st_lsm6dsv16bx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + return ret; + + ret = st_lsm6dsv16bx_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = (int)sensor->odr; + *val2 = (int)sensor->uodr; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SCALE: + switch (ch->type) { + case IIO_TEMP: + *val = 1000; + *val2 = ST_LSM6DSV16BX_TEMP_GAIN; + ret = IIO_VAL_FRACTIONAL; + break; + case IIO_ACCEL: + case IIO_ANGL_VEL: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + return -EINVAL; + } + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lsm6dsv16bx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsv16bx_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + int todr, tuodr; + u8 data; + + err = st_lsm6dsv16bx_get_odr_val(sensor->id, val, val2, + &todr, &tuodr, &data); + if (!err) { + sensor->odr = val; + sensor->uodr = tuodr; + + /* + * VTS test testSamplingRateHotSwitchOperation + * not toggle the enable status of sensor after + * changing the ODR -> force it + */ + if (sensor->hw->enable_mask & BIT(sensor->id)) { + switch (sensor->id) { + case ST_LSM6DSV16BX_ID_GYRO: + case ST_LSM6DSV16BX_ID_ACC: + err = st_lsm6dsv16bx_set_odr(sensor, + sensor->odr, + sensor->uodr); + if (err < 0) + break; + + err = st_lsm6dsv16bx_update_batching(iio_dev, 1); + default: + break; + } + } + } + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : 0; +} + +static ssize_t +st_lsm6dsv16bx_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dsv16bx_odr_table[id].size; i++) { + if (!st_lsm6dsv16bx_odr_table[id].odr_avl[i].hz) + continue; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lsm6dsv16bx_odr_table[id].odr_avl[i].hz, + st_lsm6dsv16bx_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lsm6dsv16bx_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int i, len = 0; + + for (i = 0; i < hw->fs_table[id].size; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + hw->fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static __maybe_unused int st_lsm6dsv16bx_reg_access(struct iio_dev *iio_dev, + unsigned int reg, + unsigned int writeval, + unsigned int *readval) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int ret; + + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + return ret; + + if (readval == NULL) + ret = regmap_write(sensor->hw->regmap, reg, writeval); + else + ret = regmap_read(sensor->hw->regmap, reg, readval); + + iio_device_release_direct_mode(iio_dev); + + return (ret < 0) ? ret : 0; +} + +static int st_lsm6dsv16bx_of_get_pin(struct st_lsm6dsv16bx_hw *hw, int *pin) +{ + struct device_node *np = hw->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,int-pin", pin); +} + +static int st_lsm6dsv16bx_get_int_reg(struct st_lsm6dsv16bx_hw *hw, + u8 *drdy_reg, u8 *ef_irq_reg) +{ + int int_pin; + + if (st_lsm6dsv16bx_of_get_pin(hw, &int_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + int_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (int_pin) { + case 1: + *drdy_reg = ST_LSM6DSV16BX_REG_INT1_CTRL_ADDR; + break; + case 2: + *drdy_reg = ST_LSM6DSV16BX_REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + + return -EINVAL; + } + + hw->int_pin = int_pin; + + return 0; +} + +static int +st_lsm6dsv16bx_set_selftest(struct st_lsm6dsv16bx_sensor *sensor, int index) +{ + u8 mask; + + switch (sensor->id) { + case ST_LSM6DSV16BX_ID_ACC: + mask = ST_LSM6DSV16BX_ST_XL_MASK; + break; + case ST_LSM6DSV16BX_ID_GYRO: + mask = ST_LSM6DSV16BX_ST_G_MASK; + break; + default: + return -EINVAL; + } + + return st_lsm6dsv16bx_write_with_mask(sensor->hw, + ST_LSM6DSV16BX_REG_CTRL10_ADDR, mask, + st_lsm6dsv16bx_selftest_table[index].value); +} + +static ssize_t st_lsm6dsv16bx_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6dsv16bx_selftest_table[1].smode, + st_lsm6dsv16bx_selftest_table[2].smode); +} + +static ssize_t +st_lsm6dsv16bx_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + int8_t result; + char *message; + + if (id != ST_LSM6DSV16BX_ID_ACC && + id != ST_LSM6DSV16BX_ID_GYRO) + return -EINVAL; + + result = sensor->selftest_status; + if (result == 0) + message = "na"; + else if (result < 0) + message = "fail"; + else if (result > 0) + message = "pass"; + + return sprintf(buf, "%s\n", message); +} + +static int st_lsm6dsv16bx_selftest_sensor(struct st_lsm6dsv16bx_sensor *sensor, + int test) +{ + int x_selftest = 0, y_selftest = 0, z_selftest = 0; + int x = 0, y = 0, z = 0, try_count = 0; + u8 i, status, n = 0; + u8 reg, bitmask; + int ret, delay; + u8 raw_data[6]; + + switch (sensor->id) { + case ST_LSM6DSV16BX_ID_ACC: + reg = ST_LSM6DSV16BX_REG_OUTX_L_A_ADDR; + bitmask = ST_LSM6DSV16BX_XLDA_MASK; + break; + case ST_LSM6DSV16BX_ID_GYRO: + reg = ST_LSM6DSV16BX_REG_OUTX_L_G_ADDR; + bitmask = ST_LSM6DSV16BX_GDA_MASK; + break; + default: + return -EINVAL; + } + + /* set selftest normal mode */ + ret = st_lsm6dsv16bx_set_selftest(sensor, 0); + if (ret < 0) + return ret; + + ret = st_lsm6dsv16bx_sensor_set_enable(sensor, true); + if (ret < 0) + return ret; + + /* calculate delay time because self test is running in polling mode */ + delay = 1100000 / sensor->odr; + + /* power up, wait 100 ms for stable output */ + msleep(100); + + /* for 5 times, after checking status bit, read the output registers */ + for (i = 0; i < 5; i++) { + try_count = 0; + while (try_count < 3) { + usleep_range(delay, delay + 1); + ret = st_lsm6dsv16bx_read_locked(sensor->hw, + ST_LSM6DSV16BX_REG_STATUS_REG_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsv16bx_read_locked(sensor->hw, + reg, raw_data, + sizeof(raw_data)); + if (ret < 0) + goto selftest_failure; + + /* + * for 5 times, after checking status bit, + * read the output registers + */ + x += ((s16)*(u16 *)&raw_data[0]) / 5; + y += ((s16)*(u16 *)&raw_data[2]) / 5; + z += ((s16)*(u16 *)&raw_data[4]) / 5; + n++; + break; + } + try_count++; + } + } + + if (i != n) { + dev_err(sensor->hw->dev, + "some samples missing (expected %d, read %d)\n", + i, n); + ret = -1; + + goto selftest_failure; + } + + n = 0; + + /* set selftest mode */ + st_lsm6dsv16bx_set_selftest(sensor, test); + + /* wait 100 ms for stable output */ + msleep(100); + + /* for 5 times, after checking status bit, read the output registers */ + for (i = 0; i < 5; i++) { + try_count = 0; + while (try_count < 3) { + usleep_range(delay, delay + 1); + ret = st_lsm6dsv16bx_read_locked(sensor->hw, + ST_LSM6DSV16BX_REG_STATUS_REG_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsv16bx_read_locked(sensor->hw, + reg, raw_data, + sizeof(raw_data)); + if (ret < 0) + goto selftest_failure; + + x_selftest += ((s16)*(u16 *)&raw_data[0]) / 5; + y_selftest += ((s16)*(u16 *)&raw_data[2]) / 5; + z_selftest += ((s16)*(u16 *)&raw_data[4]) / 5; + n++; + break; + } + try_count++; + } + } + + if (i != n) { + dev_err(sensor->hw->dev, + "some samples missing (expected %d, read %d)\n", + i, n); + ret = -1; + + goto selftest_failure; + } + + if ((abs(x_selftest - x) < sensor->min_st) || + (abs(x_selftest - x) > sensor->max_st)) { + sensor->selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < sensor->min_st) || + (abs(y_selftest - y) > sensor->max_st)) { + sensor->selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < sensor->min_st) || + (abs(z_selftest - z) > sensor->max_st)) { + sensor->selftest_status = -1; + goto selftest_failure; + } + + sensor->selftest_status = 1; + +selftest_failure: + /* restore selftest to normal mode */ + st_lsm6dsv16bx_set_selftest(sensor, 0); + + return st_lsm6dsv16bx_sensor_set_enable(sensor, false); +} + +static ssize_t st_lsm6dsv16bx_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + enum st_lsm6dsv16bx_sensor_id id = sensor->id; + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + u8 drdy_reg, ef_irq_reg; + int ret, test; + int odr, uodr; + u32 gain; + + if (id != ST_LSM6DSV16BX_ID_ACC && + id != ST_LSM6DSV16BX_ID_GYRO) + return -EINVAL; + + for (test = 0; test < ARRAY_SIZE(st_lsm6dsv16bx_selftest_table); test++) { + if (strncmp(buf, st_lsm6dsv16bx_selftest_table[test].smode, + strlen(st_lsm6dsv16bx_selftest_table[test].smode)) == 0) + break; + } + + if (test == ARRAY_SIZE(st_lsm6dsv16bx_selftest_table)) + return -EINVAL; + + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + return ret; + + /* self test mode unavailable if sensor enabled */ + if (hw->enable_mask & BIT(id)) { + ret = -EBUSY; + + goto out_claim; + } + + /* disable interrupt on FIFO watermak */ + ret = st_lsm6dsv16bx_get_int_reg(hw, &drdy_reg, &ef_irq_reg); + if (ret < 0) + goto restore_regs; + + ret = st_lsm6dsv16bx_write_with_mask(hw, drdy_reg, + ST_LSM6DSV16BX_INT_FIFO_TH_MASK, + 0); + if (ret < 0) + goto restore_regs; + + gain = sensor->gain; + odr = sensor->odr; + uodr = sensor->uodr; + if (id == ST_LSM6DSV16BX_ID_ACC) { + /* set BDU = 1, FS = 4 g, ODR = 60 Hz */ + st_lsm6dsv16bx_set_full_scale(sensor, + ST_LSM6DSV16BX_ACC_FS_4G_GAIN); + st_lsm6dsv16bx_set_odr(sensor, 60, 0); + } else { + /* set BDU = 1, ODR = 240 Hz, FS = 2000 dps */ + st_lsm6dsv16bx_set_full_scale(sensor, + ST_LSM6DSV16BX_GYRO_FS_2000_GAIN); + st_lsm6dsv16bx_set_odr(sensor, 240, 0); + } + + /* run test */ + st_lsm6dsv16bx_selftest_sensor(sensor, test); + +restore_regs: + /* restore configuration after test */ + st_lsm6dsv16bx_set_full_scale(sensor, gain); + st_lsm6dsv16bx_set_odr(sensor, odr, uodr); + st_lsm6dsv16bx_write_with_mask(hw, drdy_reg, + ST_LSM6DSV16BX_INT_FIFO_TH_MASK, 1); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +ssize_t st_lsm6dsv16bx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +ssize_t st_lsm6dsv16bx_get_en_tdm(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->hw->en_tdm); +} + +ssize_t st_lsm6dsv16bx_set_en_tdm(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_enable_tdm(sensor, val > 0 ? true : false); + if (err < 0) + goto out; + + sensor->hw->en_tdm = val > 0 ? true : false; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsv16bx_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dsv16bx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dsv16bx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_lsm6dsv16bx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsv16bx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, + st_lsm6dsv16bx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_lsm6dsv16bx_get_watermark, + st_lsm6dsv16bx_set_watermark, 0); +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lsm6dsv16bx_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_lsm6dsv16bx_sysfs_get_selftest_status, + st_lsm6dsv16bx_sysfs_start_selftest, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsv16bx_get_module_id, NULL, 0); +static IIO_DEVICE_ATTR(en_tdm, 0644, + st_lsm6dsv16bx_get_en_tdm, + st_lsm6dsv16bx_set_en_tdm, 0); + +static struct attribute *st_lsm6dsv16bx_acc_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + &iio_dev_attr_en_tdm.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_acc_attribute_group = { + .attrs = st_lsm6dsv16bx_acc_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_acc_info = { + .attrs = &st_lsm6dsv16bx_acc_attribute_group, + .read_raw = st_lsm6dsv16bx_read_raw, + .write_raw_get_fmt = st_lsm6dsv16bx_write_raw_get_fmt, + .write_raw = st_lsm6dsv16bx_write_raw, + .debugfs_reg_access = st_lsm6dsv16bx_reg_access, +}; + +static struct attribute *st_lsm6dsv16bx_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_gyro_attribute_group = { + .attrs = st_lsm6dsv16bx_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_gyro_info = { + .attrs = &st_lsm6dsv16bx_gyro_attribute_group, + .read_raw = st_lsm6dsv16bx_read_raw, + .write_raw_get_fmt = st_lsm6dsv16bx_write_raw_get_fmt, + .write_raw = st_lsm6dsv16bx_write_raw, +}; + +static struct attribute *st_lsm6dsv16bx_temp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_temp_scale_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_temp_attribute_group = { + .attrs = st_lsm6dsv16bx_temp_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_temp_info = { + .attrs = &st_lsm6dsv16bx_temp_attribute_group, + .read_raw = st_lsm6dsv16bx_read_raw, + .write_raw_get_fmt = st_lsm6dsv16bx_write_raw_get_fmt, + .write_raw = st_lsm6dsv16bx_write_raw, +}; + +static struct attribute *st_lsm6dsv16bx_sflp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_sflp_attribute_group = { + .attrs = st_lsm6dsv16bx_sflp_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_sflp_info = { + .attrs = &st_lsm6dsv16bx_sflp_attribute_group, + .read_raw = st_lsm6dsv16bx_read_raw, + .write_raw = st_lsm6dsv16bx_write_raw, +}; + +static const unsigned long st_lsm6dsv16bx_available_scan_masks[] = { + GENMASK(3, 0), 0x0 +}; + +static const unsigned long st_lsm6dsv16bx_temp_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int st_lsm6dsv16bx_reset_device(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + /* sw reset */ + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_CTRL3_ADDR, + ST_LSM6DSV16BX_SW_RESET_MASK, + 1); + if (err < 0) + return err; + + msleep(10); + + /* boot */ + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_CTRL3_ADDR, + ST_LSM6DSV16BX_BOOT_MASK, 1); + + msleep(50); + + return err; +} + +/* from AN5845 */ +static int st_lsm6dsv16bx_sflp_init_device(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + return err; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_PAGE_RW_ADDR, + ST_LSM6DSV16BX_PAGE_WRITE_MASK, + 1); + if (err < 0) + return err; + + err = regmap_write(hw->regmap, ST_LSM6DSV16BX_REG_PAGE_ADDRESS_ADDR, 0xd2); + if (err < 0) + return err; + + err = regmap_write(hw->regmap, ST_LSM6DSV16BX_REG_PAGE_VALUE_ADDR, 0x50); + if (err < 0) + return err; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_PAGE_RW_ADDR, + ST_LSM6DSV16BX_PAGE_WRITE_MASK, + 0); + if (err < 0) + return err; + + return st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); +} + +/* from AN5845 */ +static int st_lsm6dsv16bx_tdm_init_device(struct st_lsm6dsv16bx_hw *hw) +{ + int tdm_wclk, tdm_slot_sel, tdm_ord_sel, tdm_fs_xl, tdm_wclk_bclk_sel; + struct device_node *np = hw->dev->of_node; + int err; + + if (!np) + return -EINVAL; + + err = of_property_read_u32(np, "tdm_wclk", &tdm_wclk); + if (err < 0) { + /* use default wclk 8 kHz */ + tdm_wclk = 0x01; + tdm_wclk_bclk_sel = 0; + } + + switch (tdm_wclk) { + case 0: + tdm_wclk_bclk_sel = 1; + break; + case 1: + tdm_wclk_bclk_sel = 0; + break; + default: + dev_err(hw->dev, "invalid tdm_wclk value %d\n", tdm_wclk); + + return -EINVAL; + } + + err = of_property_read_u32(np, "tdm_slot_sel", &tdm_slot_sel); + if (err < 0) + /* use default slot {0, 1, 2} */ + tdm_slot_sel = 0x0; + + if (tdm_slot_sel < 0 || tdm_slot_sel > 1) { + dev_err(hw->dev, "invalid tdm_slot_sel value %d\n", tdm_slot_sel); + + return -EINVAL; + } + + err = of_property_read_u32(np, "tdm_ord_sel", &tdm_ord_sel); + if (err < 0) + /* use default X, Y, Z order */ + tdm_ord_sel = 0x02; + + if (tdm_ord_sel < 0 || tdm_ord_sel > 2) { + dev_err(hw->dev, "invalid tdm_ord_sel value %d\n", tdm_ord_sel); + + return -EINVAL; + } + + err = of_property_read_u32(np, "tdm_fs_xl", &tdm_fs_xl); + if (err < 0) + /* use default ±4 g */ + tdm_fs_xl = 0x01; + + if (tdm_fs_xl < 0 || tdm_fs_xl > 2) { + dev_err(hw->dev, "invalid tdm_fs_xl value %d\n", tdm_fs_xl); + + return -EINVAL; + } + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_TDM_CFG0_ADDR, + ST_LSM6DSV16BX_REG_TDM_WCLK_MASK, + tdm_wclk); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_TDM_CFG0_ADDR, + ST_LSM6DSV16BX_REG_TDM_WCLK_BCLK_SEL_MASK, + tdm_wclk_bclk_sel); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_TDM_CFG0_ADDR, + ST_LSM6DSV16BX_REG_TDM_SLOT_SEL_MASK, + tdm_slot_sel); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_TDM_CFG1_ADDR, + ST_LSM6DSV16BX_REG_TDM_AXES_ORD_SEL_MASK, + tdm_ord_sel); + if (err < 0) + return err; + + return st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_TDM_CFG2_ADDR, + ST_LSM6DSV16BX_REG_TDM_FS_XL_MASK, + tdm_fs_xl); +} + +static int st_lsm6dsv16bx_init_device(struct st_lsm6dsv16bx_hw *hw) +{ + u8 drdy_reg, ef_irq_reg; + int err; + + /* latch interrupts */ + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_TAP_CFG0_ADDR, + ST_LSM6DSV16BX_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable Block Data Update */ + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_CTRL3_ADDR, + ST_LSM6DSV16BX_BDU_MASK, 1); + if (err < 0) + return err; + + /* init timestamp engine */ + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_FUNCTIONS_ENABLE_ADDR, + ST_LSM6DSV16BX_TIMESTAMP_EN_MASK, 1); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_get_int_reg(hw, &drdy_reg, &ef_irq_reg); + if (err < 0) + return err; + + /* enable DRDY MASK for filters settling time */ + err = st_lsm6dsv16bx_write_with_mask(hw, ST_LSM6DSV16BX_REG_CTRL4_ADDR, + ST_LSM6DSV16BX_DRDY_MASK, 1); + if (err < 0) + return err; + + /* check for sflp support before init */ + if (hw->settings->st_sflp_probe) { + err = st_lsm6dsv16bx_sflp_init_device(hw); + if (err < 0) + return err; + } + + if (hw->settings->st_tdm_probe) { + err = st_lsm6dsv16bx_tdm_init_device(hw); + if (err < 0) + return err; + } + + /* enable FIFO watermak interrupt */ + return st_lsm6dsv16bx_write_with_mask(hw, drdy_reg, + ST_LSM6DSV16BX_INT_FIFO_TH_MASK, + 1); +} + +static struct iio_dev * +st_lsm6dsv16bx_alloc_iiodev(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = hw->dev; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->watermark = 1; + + sensor->decimator = 0; + sensor->dec_counter = 0; + sensor->last_fifo_timestamp = 0; + + switch (id) { + case ST_LSM6DSV16BX_ID_ACC: + iio_dev->channels = st_lsm6dsv16bx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_accel", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_acc_info; + iio_dev->available_scan_masks = st_lsm6dsv16bx_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSV16BX_REG_FIFO_CTRL3_ADDR; + sensor->batch_reg.mask = ST_LSM6DSV16BX_BDR_XL_MASK; + sensor->max_watermark = ST_LSM6DSV16BX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsv16bx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsv16bx_odr_table[id].odr_avl[1].uhz; + sensor->gain = hw->fs_table[id].fs_avl[0].gain; + sensor->min_st = ST_LSM6DSV16BX_SELFTEST_ACCEL_MIN; + sensor->max_st = ST_LSM6DSV16BX_SELFTEST_ACCEL_MAX; + break; + case ST_LSM6DSV16BX_ID_GYRO: + iio_dev->channels = st_lsm6dsv16bx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_gyro_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_gyro", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_gyro_info; + iio_dev->available_scan_masks = st_lsm6dsv16bx_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSV16BX_REG_FIFO_CTRL3_ADDR; + sensor->batch_reg.mask = ST_LSM6DSV16BX_BDR_GY_MASK; + sensor->max_watermark = ST_LSM6DSV16BX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsv16bx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsv16bx_odr_table[id].odr_avl[1].uhz; + sensor->gain = hw->fs_table[id].fs_avl[1].gain; + sensor->min_st = ST_LSM6DSV16BX_SELFTEST_GYRO_MIN; + sensor->max_st = ST_LSM6DSV16BX_SELFTEST_GYRO_MAX; + break; + case ST_LSM6DSV16BX_ID_TEMP: + iio_dev->channels = st_lsm6dsv16bx_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_temp_info; + iio_dev->available_scan_masks = + st_lsm6dsv16bx_temp_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSV16BX_REG_FIFO_CTRL4_ADDR; + sensor->batch_reg.mask = ST_LSM6DSV16BX_ODR_T_BATCH_MASK; + sensor->max_watermark = ST_LSM6DSV16BX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsv16bx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsv16bx_odr_table[id].odr_avl[1].uhz; + sensor->gain = hw->fs_table[id].fs_avl[1].gain; + sensor->offset = ST_LSM6DSV16BX_TEMP_OFFSET; + break; + case ST_LSM6DSV16BX_ID_6X_GAME: + iio_dev->channels = st_lsm6dsv16bx_sflp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_sflp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_gamerot", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_sflp_info; + iio_dev->available_scan_masks = st_lsm6dsv16bx_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSV16BX_REG_SFLP_ODR_ADDR; + sensor->batch_reg.mask = ST_LSM6DSV16BX_SFLP_GAME_ODR_MASK; + sensor->max_watermark = ST_LSM6DSV16BX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsv16bx_odr_table[id].odr_avl[3].hz; + sensor->uodr = st_lsm6dsv16bx_odr_table[id].odr_avl[3].uhz; + sensor->gain = 1; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + st_lsm6dsv16bx_set_full_scale(sensor, sensor->gain); + + return iio_dev; +} + +static void st_lsm6dsv16bx_disable_regulator_action(void *_data) +{ + struct st_lsm6dsv16bx_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static void st_lsm6dsv16bx_get_properties(struct st_lsm6dsv16bx_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +static int st_lsm6dsv16bx_power_enable(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + hw->vdd_supply = devm_regulator_get(hw->dev, "vdd"); + if (IS_ERR(hw->vdd_supply)) { + if (PTR_ERR(hw->vdd_supply) != -EPROBE_DEFER) + dev_err(hw->dev, "Failed to get vdd regulator %d\n", + (int)PTR_ERR(hw->vdd_supply)); + + return PTR_ERR(hw->vdd_supply); + } + + hw->vddio_supply = devm_regulator_get(hw->dev, "vddio"); + if (IS_ERR(hw->vddio_supply)) { + if (PTR_ERR(hw->vddio_supply) != -EPROBE_DEFER) + dev_err(hw->dev, "Failed to get vddio regulator %d\n", + (int)PTR_ERR(hw->vddio_supply)); + + return PTR_ERR(hw->vddio_supply); + } + + err = regulator_enable(hw->vdd_supply); + if (err) { + dev_err(hw->dev, "Failed to enable vdd regulator: %d\n", err); + + return err; + } + + err = regulator_enable(hw->vddio_supply); + if (err) { + regulator_disable(hw->vdd_supply); + + return err; + } + + err = devm_add_action_or_reset(hw->dev, + st_lsm6dsv16bx_disable_regulator_action, + hw); + if (err) { + dev_err(hw->dev, + "Failed to setup regulator cleanup action %d\n", err); + + return err; + } + + return 0; +} + +int st_lsm6dsv16bx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap) +{ + struct st_lsm6dsv16bx_hw *hw; + int i, err; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + + mutex_init(&hw->lock); + mutex_init(&hw->fifo_lock); + mutex_init(&hw->page_lock); + + hw->dev = dev; + hw->irq = irq; + hw->regmap = regmap; + + err = st_lsm6dsv16bx_power_enable(hw); + if (err != 0) + return err; + + /* select register bank zero */ + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, 0); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_check_whoami(hw, hw_id); + if (err < 0) + return err; + + st_lsm6dsv16bx_get_properties(hw); + + err = st_lsm6dsv16bx_get_odr_calibration(hw); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_reset_device(hw); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_init_device(hw); + if (err < 0) + return err; + +#if KERNEL_VERSION(5, 15, 0) <= LINUX_VERSION_CODE + err = iio_read_mount_matrix(dev, &hw->orientation); +#elif KERNEL_VERSION(5, 2, 0) <= LINUX_VERSION_CODE + err = iio_read_mount_matrix(dev, "mount-matrix", &hw->orientation); +#else /* LINUX_VERSION_CODE */ + err = of_iio_read_mount_matrix(dev, "mount-matrix", &hw->orientation); +#endif /* LINUX_VERSION_CODE */ + + if (err) { + dev_err(dev, "Failed to retrieve mounting matrix %d\n", err); + return err; + } + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_main_sensor_list); i++) { + enum st_lsm6dsv16bx_sensor_id id = st_lsm6dsv16bx_main_sensor_list[i]; + + /* don't probe if sflp not supported */ + if (!hw->settings->st_sflp_probe && + (id == ST_LSM6DSV16BX_ID_6X_GAME)) + continue; + + hw->iio_devs[id] = st_lsm6dsv16bx_alloc_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + /* allocate step counter before buffer setup because use FIFO */ + err = st_lsm6dsv16bx_probe_embfunc(hw); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_probe_event(hw); + if (err < 0) + return err; + + if (hw->settings->st_qvar_probe && + (!dev_fwnode(dev) || + device_property_read_bool(dev, "enable-qvar"))) { + err = st_lsm6dsv16bx_qvar_probe(hw); + if (err) + return err; + } + + if (hw->irq > 0) { + err = st_lsm6dsv16bx_buffers_setup(hw); + if (err < 0) + return err; + } + + if (st_lsm6dsv16bx_run_mlc_task(hw)) { + err = st_lsm6dsv16bx_mlc_probe(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LSM6DSV16BX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + return err; + } + + if (st_lsm6dsv16bx_run_mlc_task(hw)) { + err = st_lsm6dsv16bx_mlc_init_preload(hw); + if (err) + return err; + } + + device_init_wakeup(dev, + device_property_read_bool(dev, "wakeup-source")); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsv16bx_probe); + +static int __maybe_unused st_lsm6dsv16bx_suspend(struct device *dev) +{ + struct st_lsm6dsv16bx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSV16BX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_lsm6dsv16bx_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + + if (st_lsm6dsv16bx_is_fifo_enabled(hw)) + err = st_lsm6dsv16bx_suspend_fifo(hw); + + if (device_may_wakeup(dev)) + enable_irq_wake(hw->irq); + + dev_info(dev, "Suspending device\n"); + + return err < 0 ? err : 0; +} + +static int __maybe_unused st_lsm6dsv16bx_resume(struct device *dev) +{ + struct st_lsm6dsv16bx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Resuming device\n"); + + if (device_may_wakeup(dev)) + disable_irq_wake(hw->irq); + + for (i = 0; i < ST_LSM6DSV16BX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_lsm6dsv16bx_set_odr(sensor, sensor->odr, sensor->uodr); + if (err < 0) + return err; + } + + if (st_lsm6dsv16bx_is_fifo_enabled(hw)) + err = st_lsm6dsv16bx_set_fifo_mode(hw, + ST_LSM6DSV16BX_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_lsm6dsv16bx_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsv16bx_suspend, st_lsm6dsv16bx_resume) +}; +EXPORT_SYMBOL(st_lsm6dsv16bx_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsv16bx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_embfunc.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_embfunc.c new file mode 100644 index 000000000000..f4d020743914 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_embfunc.c @@ -0,0 +1,579 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx embedded function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +/** + * Step Counter IIO channels description + * + * Step Counter exports to IIO framework the following data channels: + * Step Counters (16 bit unsigned in little endian) + * Timestamp (64 bit signed in little endian) + * Step Counter exports to IIO framework the following event channels: + * Flush event done + */ +static const struct iio_chan_spec st_lsm6dsv16bx_step_counter_channels[] = { + { + .type = STM_IIO_STEP_COUNTER, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_STEP_COUNTER, flush), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +/** + * Step Detector IIO channels description + * + * Step Detector exports to IIO framework the following event channels: + * Step detection event detection + */ +static const struct iio_chan_spec st_lsm6dsv16bx_step_detector_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_STEPS, thr), +}; + +/** + * Significant Motion IIO channels description + * + * Significant Motion exports to IIO framework the following event + * channels: + * Significant Motion event detection + */ +static const struct iio_chan_spec st_lsm6dsv16bx_sign_motion_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_SIGN_MOTION, thr), +}; + +/** + * Tilt IIO channels description + * + * Tilt exports to IIO framework the following event channels: + * Tilt event detection + */ +static const struct iio_chan_spec st_lsm6dsv16bx_tilt_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_TILT, thr), +}; + +static const unsigned long st_lsm6dsv16bx_embfunc_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int +st_lsm6dsv16bx_embfunc_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + u8 mask, u8 irq_mask, bool enable) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSV16BX_REG_EMB_FUNC_INT1_ADDR : + ST_LSM6DSV16BX_REG_EMB_FUNC_INT2_ADDR; + int err; + + err = st_lsm6dsv16bx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_EN_A_ADDR, + mask, + enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsv16bx_write_with_mask(hw, int_reg, irq_mask, + enable ? 1 : 0); + +reset_page: + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); + +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsv16bx_embfunc_sensor_set_enable() - Enable Embedded Function + * sensor [EMB_FUN] + * + * @sensor: ST IMU sensor instance + * @enable: Enable/Disable sensor + * + * return < 0 if error, 0 otherwise + */ +static int +st_lsm6dsv16bx_embfunc_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + int err; + + switch (sensor->id) { + case ST_LSM6DSV16BX_ID_STEP_DETECTOR: + err = st_lsm6dsv16bx_embfunc_set_enable(sensor, + ST_LSM6DSV16BX_REG_PEDO_EN_MASK, + ST_LSM6DSV16BX_INT_STEP_DETECTOR_MASK, + enable); + break; + case ST_LSM6DSV16BX_ID_SIGN_MOTION: + err = st_lsm6dsv16bx_embfunc_set_enable(sensor, + ST_LSM6DSV16BX_REG_SIGN_MOTION_EN_MASK, + ST_LSM6DSV16BX_INT_SIG_MOT_MASK, + enable); + break; + case ST_LSM6DSV16BX_ID_TILT: + err = st_lsm6dsv16bx_embfunc_set_enable(sensor, + ST_LSM6DSV16BX_REG_TILT_EN_MASK, + ST_LSM6DSV16BX_INT_TILT_MASK, + enable); + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +/** + * st_lsm6dsv16bx_reset_step_counter() - Reset Step Counter value [EMB_FUN] + * + * @iio_dev: IIO device + * + * return < 0 if error, 0 otherwise + */ +static int st_lsm6dsv16bx_reset_step_counter(struct iio_dev *iio_dev) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock_page; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_SRC_ADDR, + ST_LSM6DSV16BX_PEDO_RST_STEP_MASK, 1); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); + +unlock_page: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsv16bx_read_embfunc_config() - Read embedded function sensor + * event configuration + * + * @iio_dev: IIO Device. + * @chan: IIO Channel. + * @type: Event Type. + * @dir: Event Direction. + * + * return 1 if Enabled, 0 Disabled + */ +static int +st_lsm6dsv16bx_read_embfunc_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/** + * st_lsm6dsv16bx_write_embfunc_config() - Write embedded function + * sensor event configuration + * + * @iio_dev: IIO Device. + * @chan: IIO Channel. + * @type: Event Type. + * @dir: Event Direction. + * @state: New event state. + * + * return 0 if OK, negative for ERROR + */ +static int +st_lsm6dsv16bx_write_embfunc_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + int state) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_lsm6dsv16bx_embfunc_sensor_set_enable(sensor, state); + iio_device_release_direct_mode(iio_dev); + + return err; +} + +/** + * st_lsm6dsv16bx_sysfs_reset_step_counter() - Reset step counter value + * + * @dev: IIO Device. + * @attr: IIO Channel attribute. + * @buf: User buffer. + * @size: User buffer size. + * + * return buffer len, negative for ERROR + */ +static ssize_t +st_lsm6dsv16bx_sysfs_reset_step_counter(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_lsm6dsv16bx_reset_step_counter(iio_dev); + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static IIO_DEVICE_ATTR(reset_stepc, 0200, NULL, + st_lsm6dsv16bx_sysfs_reset_step_counter, 0); + +static IIO_DEVICE_ATTR(hwfifo_stepc_watermark_max, 0444, + st_lsm6dsv16bx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_stepc_flush, 0200, NULL, + st_lsm6dsv16bx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_stepc_watermark, 0644, + st_lsm6dsv16bx_get_watermark, + st_lsm6dsv16bx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsv16bx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsv16bx_step_counter_attributes[] = { + &iio_dev_attr_hwfifo_stepc_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_stepc_watermark.dev_attr.attr, + &iio_dev_attr_reset_stepc.dev_attr.attr, + &iio_dev_attr_hwfifo_stepc_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_lsm6dsv16bx_step_counter_attribute_group = { + .attrs = st_lsm6dsv16bx_step_counter_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_step_counter_info = { + .attrs = &st_lsm6dsv16bx_step_counter_attribute_group, +}; + +static struct attribute *st_lsm6dsv16bx_step_detector_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_lsm6dsv16bx_step_detector_attribute_group = { + .attrs = st_lsm6dsv16bx_step_detector_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_step_detector_info = { + .attrs = &st_lsm6dsv16bx_step_detector_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_embfunc_config, + .write_event_config = st_lsm6dsv16bx_write_embfunc_config, +}; + +static struct attribute *st_lsm6dsv16bx_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_lsm6dsv16bx_sign_motion_attribute_group = { + .attrs = st_lsm6dsv16bx_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_sign_motion_info = { + .attrs = &st_lsm6dsv16bx_sign_motion_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_embfunc_config, + .write_event_config = st_lsm6dsv16bx_write_embfunc_config, +}; + +static struct attribute *st_lsm6dsv16bx_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_tilt_attribute_group = { + .attrs = st_lsm6dsv16bx_tilt_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_tilt_info = { + .attrs = &st_lsm6dsv16bx_tilt_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_embfunc_config, + .write_event_config = st_lsm6dsv16bx_write_embfunc_config, +}; + +static int st_lsm6dsv16bx_embfunc_init(struct st_lsm6dsv16bx_hw *hw) +{ + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSV16BX_REG_MD1_CFG_ADDR : + ST_LSM6DSV16BX_REG_MD2_CFG_ADDR; + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock_page; + + /* enable embedded function latched interrupt */ + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_PAGE_RW_ADDR, + ST_LSM6DSV16BX_EMB_FUNC_LIR_MASK, 1); + if (err < 0) + goto unlock_page; + + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); + + /* enable embedded function interrupt by default */ + err = __st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_REG_INT_EMB_FUNC_MASK, + 1); +unlock_page: + mutex_unlock(&hw->page_lock); + + return err; +} + +static struct iio_dev * +st_lsm6dsv16bx_alloc_embfunc_iiodev(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = hw->dev; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->watermark = 1; + iio_dev->available_scan_masks = + st_lsm6dsv16bx_embfunc_available_scan_masks; + + /* set main sensor odr to 26 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[2].hz; + switch (id) { + case ST_LSM6DSV16BX_ID_STEP_COUNTER: + iio_dev->channels = st_lsm6dsv16bx_step_counter_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_step_counter_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_stepc", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_step_counter_info; + break; + case ST_LSM6DSV16BX_ID_STEP_DETECTOR: + iio_dev->channels = st_lsm6dsv16bx_step_detector_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_step_detector_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_stepd", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_step_detector_info; + break; + case ST_LSM6DSV16BX_ID_SIGN_MOTION: + iio_dev->channels = st_lsm6dsv16bx_sign_motion_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_sign_motion_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sigmot", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_sign_motion_info; + break; + case ST_LSM6DSV16BX_ID_TILT: + iio_dev->channels = st_lsm6dsv16bx_tilt_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_tilt_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tilt", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_tilt_info; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/** + * st_lsm6dsv16bx_step_counter_set_enable() - Enable Step Counter + * Sensor [EMB_FUN] + * + * @sensor: ST IMU sensor instance + * @enable: Enable/Disable sensor + * + * return < 0 if error, 0 otherwise + */ +int st_lsm6dsv16bx_step_counter_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int err; + + err = st_lsm6dsv16bx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock; + + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_EN_A_ADDR, + ST_LSM6DSV16BX_REG_PEDO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + /* enable step counter batching in fifo */ + err = __st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_FIFO_EN_A_ADDR, + ST_LSM6DSV16BX_STEP_COUNTER_FIFO_EN_MASK, + enable); + +reset_page: + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsv16bx_embfunc_handler_thread() - Bottom handler for embedded + * function event detection + * + * @hw: ST IMU MEMS hw instance. + * + * return IRQ_HANDLED or < 0 for error + */ +int st_lsm6dsv16bx_embfunc_handler_thread(struct st_lsm6dsv16bx_hw *hw) +{ + if (hw->enable_mask & (BIT(ST_LSM6DSV16BX_ID_STEP_DETECTOR) | + BIT(ST_LSM6DSV16BX_ID_SIGN_MOTION) | + BIT(ST_LSM6DSV16BX_ID_TILT))) { + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + err = st_lsm6dsv16bx_read_locked(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_STATUS_MAINPAGE_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* embedded function sensors */ + if (status & ST_LSM6DSV16BX_IS_STEP_DET_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_STEP_DETECTOR]; + event = IIO_UNMOD_EVENT_CODE(IIO_STEPS, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LSM6DSV16BX_IS_SIGMOT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_SIGN_MOTION]; + event = IIO_UNMOD_EVENT_CODE(STM_IIO_SIGN_MOTION, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LSM6DSV16BX_IS_TILT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_TILT]; + event = IIO_UNMOD_EVENT_CODE(STM_IIO_TILT, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + } + + return IRQ_HANDLED; +} + +/** + * st_lsm6dsv16bx_probe_embfunc() - Allocate IIO embedded function device + * + * @hw: ST IMU MEMS hw instance. + * + * return 0 or < 0 for error + */ +int st_lsm6dsv16bx_probe_embfunc(struct st_lsm6dsv16bx_hw *hw) +{ + enum st_lsm6dsv16bx_sensor_id id; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_embfunc_sensor_list); + i++) { + + id = st_lsm6dsv16bx_embfunc_sensor_list[i]; + hw->iio_devs[id] = st_lsm6dsv16bx_alloc_embfunc_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + return st_lsm6dsv16bx_embfunc_init(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_events.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_events.c new file mode 100644 index 000000000000..33bb98e55f86 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_events.c @@ -0,0 +1,1035 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx events function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +static const struct st_lsm6dsv16bx_ff_th st_lsm6dsv16bx_free_fall_threshold[] = { + [0] = { .val = 0x00, .mg = 156 }, + [1] = { .val = 0x01, .mg = 219 }, + [2] = { .val = 0x02, .mg = 250 }, + [3] = { .val = 0x03, .mg = 312 }, + [4] = { .val = 0x04, .mg = 344 }, + [5] = { .val = 0x05, .mg = 406 }, + [6] = { .val = 0x06, .mg = 469 }, + [7] = { .val = 0x07, .mg = 500 }, +}; + +static const struct st_lsm6dsv16bx_6D_th st_lsm6dsv16bx_6D_threshold[] = { + [0] = { .val = 0x00, .deg = 80 }, + [1] = { .val = 0x01, .deg = 70 }, + [2] = { .val = 0x02, .deg = 60 }, + [3] = { .val = 0x03, .deg = 50 }, +}; +static const unsigned long st_lsm6dsv16bx_event_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_wk_channels[] = { + { + .type = STM_IIO_GESTURE, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 8, + .storagebits = 8, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_ff_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_sc_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_6D_channels[] = { + { + .type = STM_IIO_GESTURE, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 8, + .storagebits = 8, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_tap_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_TAP, thr), +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_dtap_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(STM_IIO_TAP_TAP, thr), +}; + +/* + * st_lsm6dsv16bx_set_wake_up_thershold - set wake-up threshold in ug + * + * @hw - ST IMU MEMS hw instance + * @th_ug - wake-up threshold in ug (micro g) + * + * wake-up threshold register val = (th_ug * 2 ^ 6) / (1000000 * FS_XL) + */ +static int st_lsm6dsv16bx_set_wake_up_thershold(struct st_lsm6dsv16bx_hw *hw, + int th_ug) +{ + struct st_lsm6dsv16bx_sensor *sensor; + u8 fs_xl_g[] = { 2, 16, 4, 8 }; + struct iio_dev *iio_dev; + u8 val, fs_xl, max_th; + int tmp, err; + + err = st_lsm6dsv16bx_read_with_mask(hw, + hw->fs_table[ST_LSM6DSV16BX_ID_ACC].reg.addr, + hw->fs_table[ST_LSM6DSV16BX_ID_ACC].reg.mask, + &fs_xl); + if (err < 0) + return err; + + if (fs_xl >= ARRAY_SIZE(fs_xl_g)) + return -EINVAL; + + tmp = (th_ug * 64) / (fs_xl_g[fs_xl] * 1000000); + + val = (u8)tmp; + max_th = ST_LSM6DSV16BX_WK_THS_MASK >> __ffs(ST_LSM6DSV16BX_WK_THS_MASK); + if (val > max_th) + val = max_th; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSV16BX_WK_THS_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[0] = th_ug; + + return 0; +} + +/* + * st_lsm6dsv16bx_set_wake_up_duration - set wake-up duration in ms + * + * @hw - ST IMU MEMS hw instance + * @dur_ms - wake-up duration in ms + * + * wake-up duration register val is related to XL ODR + */ +static int st_lsm6dsv16bx_set_wake_up_duration(struct st_lsm6dsv16bx_hw *hw, + int dur_ms) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + int i, tmp, sensor_odr, err; + u8 val, odr_xl, max_dur; + + err = st_lsm6dsv16bx_read_with_mask(hw, + hw->odr_table[ST_LSM6DSV16BX_ID_ACC].reg.addr, + hw->odr_table[ST_LSM6DSV16BX_ID_ACC].reg.mask, + &odr_xl); + if (err < 0) + return err; + + if (odr_xl == 0) { + dev_info(hw->dev, "use default ODR (26 Hz)\n"); + odr_xl = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[2].val; + } + + for (i = 0; i < hw->odr_table[ST_LSM6DSV16BX_ID_ACC].size; i++) { + if (odr_xl == + hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[i].val) + break; + } + + if (i == hw->odr_table[ST_LSM6DSV16BX_ID_ACC].size) + return -EINVAL; + + + sensor_odr = ST_LSM6DSV16BX_ODR_EXPAND( + hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[i].hz, + hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[i].uhz); + + tmp = dur_ms / (1000000 / (sensor_odr / 1000)); + val = (u8)tmp; + max_dur = ST_LSM6DSV16BX_WAKE_DUR_MASK >> + __ffs(ST_LSM6DSV16BX_WAKE_DUR_MASK); + if (val > max_dur) + val = max_dur; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_WAKE_UP_DUR_ADDR, + ST_LSM6DSV16BX_WAKE_DUR_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[1] = dur_ms; + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[i].hz; + + return 0; +} + +/* + * st_lsm6dsv16bx_set_freefall_threshold - set free fall threshold + * detection mg + * + * @hw - ST IMU MEMS hw instance + * @th_mg - free fall threshold in mg + */ +static int st_lsm6dsv16bx_set_freefall_threshold(struct st_lsm6dsv16bx_hw *hw, + int th_mg) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_free_fall_threshold); i++) { + if (th_mg >= st_lsm6dsv16bx_free_fall_threshold[i].mg) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsv16bx_free_fall_threshold)) + return -EINVAL; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_FREE_FALL_ADDR, + ST_LSM6DSV16BX_FF_THS_MASK, + st_lsm6dsv16bx_free_fall_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_FF]; + sensor = iio_priv(iio_dev); + sensor->conf[2] = th_mg; + + return 0; +} + +/* + * st_lsm6dsv16bx_set_6D_threshold - set 6D threshold detection in degrees + * + * @hw - ST IMU MEMS hw instance + * @deg - 6D threshold in degrees + */ +static int st_lsm6dsv16bx_set_6D_threshold(struct st_lsm6dsv16bx_hw *hw, + int deg) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_6D_threshold); i++) { + if (deg >= st_lsm6dsv16bx_6D_threshold[i].deg) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsv16bx_6D_threshold)) + return -EINVAL; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_TAP_THS_6D_ADDR, + ST_LSM6DSV16BX_SIXD_THS_MASK, + st_lsm6dsv16bx_6D_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_6D]; + sensor = iio_priv(iio_dev); + sensor->conf[3] = deg; + + return 0; +} + +/* + * st_lsm6dsv16bx_init_tap - initialize tap detection to default value + * + * @hw - ST IMU MEMS hw instance + */ +static int st_lsm6dsv16bx_init_tap(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_TAP_CFG0_ADDR, + ST_LSM6DSV16BX_REG_TAP_EN_MASK, + FIELD_PREP(ST_LSM6DSV16BX_REG_TAP_EN_MASK, 0x07)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_TAP_CFG1_ADDR, + ST_LSM6DSV16BX_TAP_THS_X_MASK, + FIELD_PREP(ST_LSM6DSV16BX_TAP_THS_X_MASK, 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_TAP_CFG2_ADDR, + ST_LSM6DSV16BX_TAP_THS_Y_MASK, + FIELD_PREP(ST_LSM6DSV16BX_TAP_THS_Y_MASK, 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_TAP_THS_6D_ADDR, + ST_LSM6DSV16BX_TAP_THS_Z_MASK, + FIELD_PREP(ST_LSM6DSV16BX_TAP_THS_Z_MASK, 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_TAP_DUR_ADDR, + ST_LSM6DSV16BX_SHOCK_MASK, + FIELD_PREP(ST_LSM6DSV16BX_SHOCK_MASK, 0x02)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSV16BX_REG_TAP_DUR_ADDR, + ST_LSM6DSV16BX_QUIET_MASK, + FIELD_PREP(ST_LSM6DSV16BX_QUIET_MASK, 0x01)); + + return err < 0 ? err : 0; +} + +static int +st_lsm6dsv16bx_event_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + int err, eint = !!enable; + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSV16BX_REG_MD1_CFG_ADDR : + ST_LSM6DSV16BX_REG_MD2_CFG_ADDR; + + err = st_lsm6dsv16bx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + switch (sensor->id) { + case ST_LSM6DSV16BX_ID_WK: + err = st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_INT_WU_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSV16BX_ID_FF: + err = st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_INT_FF_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSV16BX_ID_SLPCHG: + err = st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_INT_SLEEP_CHANGE_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSV16BX_ID_6D: + err = st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_INT_6D_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSV16BX_ID_TAP: + err = st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_TAP_IA_MASK, + eint); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSV16BX_SINGLE_DOUBLE_TAP_MASK, + 0); + if (err < 0) + return err; + break; + case ST_LSM6DSV16BX_ID_DTAP: + err = st_lsm6dsv16bx_write_with_mask(hw, int_reg, + ST_LSM6DSV16BX_INT_DOUBLE_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSV16BX_SINGLE_DOUBLE_TAP_MASK, + 1); + if (err < 0) + return err; + break; + default: + err = -EINVAL; + break; + } + + if (err >= 0) { + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_FUNCTIONS_ENABLE_ADDR, + ST_LSM6DSV16BX_INTERRUPTS_ENABLE_MASK, + eint); + if (eint == 0) + hw->enable_mask &= ~BIT(sensor->id); + else + hw->enable_mask |= BIT(sensor->id); + } + + return err; +} + +static int +st_lsm6dsv16bx_read_event_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +static int +st_lsm6dsv16bx_write_event_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + int state) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_lsm6dsv16bx_event_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_lsm6dsv16bx_wakeup_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[0]); +} + +static ssize_t +st_lsm6dsv16bx_wakeup_threshold_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_set_wake_up_thershold(sensor->hw, val); + if (err < 0) + goto out; + + sensor->conf[0] = val; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static ssize_t +st_lsm6dsv16bx_wakeup_duration_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[1]); +} + +static ssize_t +st_lsm6dsv16bx_wakeup_duration_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_set_wake_up_duration(sensor->hw, val); + if (err < 0) + goto out; + + sensor->conf[1] = val; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static ssize_t +st_lsm6dsv16bx_freefall_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[2]); +} + +static ssize_t +st_lsm6dsv16bx_freefall_threshold_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_set_freefall_threshold(sensor->hw, val); + if (err < 0) + goto out; + + sensor->conf[2] = val; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static ssize_t +st_lsm6dsv16bx_6D_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[3]); +} + +static ssize_t +st_lsm6dsv16bx_6D_threshold_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_lsm6dsv16bx_set_6D_threshold(sensor->hw, val); + if (err < 0) + goto out; + + sensor->conf[3] = val; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static IIO_DEVICE_ATTR(wakeup_threshold, 0644, + st_lsm6dsv16bx_wakeup_threshold_get, + st_lsm6dsv16bx_wakeup_threshold_set, 0); + +static IIO_DEVICE_ATTR(wakeup_duration, 0644, + st_lsm6dsv16bx_wakeup_duration_get, + st_lsm6dsv16bx_wakeup_duration_set, 0); + +static IIO_DEVICE_ATTR(freefall_threshold, 0644, + st_lsm6dsv16bx_freefall_threshold_get, + st_lsm6dsv16bx_freefall_threshold_set, 0); + +static IIO_DEVICE_ATTR(sixd_threshold, 0644, + st_lsm6dsv16bx_6D_threshold_get, + st_lsm6dsv16bx_6D_threshold_set, 0); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsv16bx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsv16bx_wk_attributes[] = { + &iio_dev_attr_wakeup_threshold.dev_attr.attr, + &iio_dev_attr_wakeup_duration.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_wk_attribute_group = { + .attrs = st_lsm6dsv16bx_wk_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_wk_info = { + .attrs = &st_lsm6dsv16bx_wk_attribute_group, +}; + +static struct attribute *st_lsm6dsv16bx_ff_attributes[] = { + &iio_dev_attr_freefall_threshold.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_ff_attribute_group = { + .attrs = st_lsm6dsv16bx_ff_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_ff_info = { + .attrs = &st_lsm6dsv16bx_ff_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_event_config, + .write_event_config = st_lsm6dsv16bx_write_event_config, +}; + +static struct attribute *st_lsm6dsv16bx_sc_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_sc_attribute_group = { + .attrs = st_lsm6dsv16bx_sc_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_sc_info = { + .attrs = &st_lsm6dsv16bx_sc_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_event_config, + .write_event_config = st_lsm6dsv16bx_write_event_config, +}; + +static struct attribute *st_lsm6dsv16bx_6D_attributes[] = { + &iio_dev_attr_sixd_threshold.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_6D_attribute_group = { + .attrs = st_lsm6dsv16bx_6D_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_6D_info = { + .attrs = &st_lsm6dsv16bx_6D_attribute_group, +}; + +static struct attribute *st_lsm6dsv16bx_tap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_tap_attribute_group = { + .attrs = st_lsm6dsv16bx_tap_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_tap_info = { + .attrs = &st_lsm6dsv16bx_tap_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_event_config, + .write_event_config = st_lsm6dsv16bx_write_event_config, +}; + +static struct attribute *st_lsm6dsv16bx_dtap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsv16bx_dtap_attribute_group = { + .attrs = st_lsm6dsv16bx_dtap_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_dtap_info = { + .attrs = &st_lsm6dsv16bx_dtap_attribute_group, + .read_event_config = st_lsm6dsv16bx_read_event_config, + .write_event_config = st_lsm6dsv16bx_write_event_config, +}; + +static struct iio_dev * +st_lsm6dsv16bx_alloc_event_iiodev(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = hw->dev; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->watermark = 1; + iio_dev->available_scan_masks = st_lsm6dsv16bx_event_available_scan_masks; + + switch (id) { + case ST_LSM6DSV16BX_ID_WK: + iio_dev->channels = st_lsm6dsv16bx_wk_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_wk_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_wk", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_wk_info; + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSV16BX_ID_FF: + iio_dev->channels = st_lsm6dsv16bx_ff_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_ff_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ff", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_ff_info; + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSV16BX_ID_SLPCHG: + iio_dev->channels = st_lsm6dsv16bx_sc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_sc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sc", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_sc_info; + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSV16BX_ID_6D: + iio_dev->channels = st_lsm6dsv16bx_6D_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_6D_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_6d", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_6D_info; + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSV16BX_ID_TAP: + iio_dev->channels = st_lsm6dsv16bx_tap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_tap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tap", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_tap_info; + + /* require main sensor odr > 400 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[7].hz; + break; + case ST_LSM6DSV16BX_ID_DTAP: + iio_dev->channels = st_lsm6dsv16bx_dtap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_dtap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_dtap", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_dtap_info; + + /* require main sensor odr > 400 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSV16BX_ID_ACC].odr_avl[7].hz; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/** + * st_lsm6dsv16bx_event_handler() - Detect embedded low power event + * + * @hw: ST IMU MEMS hw instance. + * + * return IRQ_HANDLED. + * + * NOTE: Uses page_lock through the st_lsm6dsv16bx_read_locked. + */ +int st_lsm6dsv16bx_event_handler(struct st_lsm6dsv16bx_hw *hw) +{ + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + if (hw->enable_mask & + (BIT(ST_LSM6DSV16BX_ID_WK) | BIT(ST_LSM6DSV16BX_ID_FF) | + BIT(ST_LSM6DSV16BX_ID_SLPCHG) | + BIT(ST_LSM6DSV16BX_ID_6D) | BIT(ST_LSM6DSV16BX_ID_TAP) | + BIT(ST_LSM6DSV16BX_ID_DTAP))) { + err = st_lsm6dsv16bx_read_locked(hw, + ST_LSM6DSV16BX_REG_ALL_INT_SRC_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* base function sensors */ + if (status & ST_LSM6DSV16BX_TAP_IA_MASK) { + if (BIT(ST_LSM6DSV16BX_ID_TAP)) { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_TAP]; + event = IIO_UNMOD_EVENT_CODE(STM_IIO_TAP, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } else { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_DTAP]; + event = IIO_UNMOD_EVENT_CODE(STM_IIO_TAP_TAP, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + } + + if (status & ST_LSM6DSV16BX_FF_IA_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_FF]; + event = IIO_UNMOD_EVENT_CODE(STM_IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LSM6DSV16BX_WU_IA_MASK) { + struct st_lsm6dsv16bx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_WK]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + + if (status & ST_LSM6DSV16BX_SLEEP_CHANGE_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_SLPCHG]; + event = IIO_UNMOD_EVENT_CODE(STM_IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LSM6DSV16BX_D6D_IA_MASK) { + struct st_lsm6dsv16bx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LSM6DSV16BX_ID_6D]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + } + + return IRQ_HANDLED; +} + +static inline int st_lsm6dsv16bx_get_6D(struct st_lsm6dsv16bx_hw *hw, u8 *out) +{ + return st_lsm6dsv16bx_read_with_mask(hw, + ST_LSM6DSV16BX_REG_D6D_SRC_ADDR, + ST_LSM6DSV16BX_D6D_EVENT_MASK, + out); +} + +static inline int st_lsm6dsv16bx_get_wk(struct st_lsm6dsv16bx_hw *hw, u8 *out) +{ + return st_lsm6dsv16bx_read_with_mask(hw, + ST_LSM6DSV16BX_REG_WAKE_UP_SRC_ADDR, + ST_LSM6DSV16BX_WAKE_UP_EVENT_MASK, out); +} + +static irqreturn_t st_lsm6dsv16bx_6D_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + st_lsm6dsv16bx_get_6D(sensor->hw, &sensor->scan.event); + iio_push_to_buffers_with_timestamp(iio_dev, &sensor->scan.event, + iio_get_time_ns(iio_dev)); + + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +static irqreturn_t st_lsm6dsv16bx_wk_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + st_lsm6dsv16bx_get_wk(sensor->hw, &sensor->scan.event); + iio_push_to_buffers_with_timestamp(iio_dev, &sensor->scan.event, + iio_get_time_ns(iio_dev)); + + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +int st_lsm6dsv16bx_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig); + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + dev_info(sensor->hw->dev, "trigger set %d\n", state); + + return 0; +} + +static const struct iio_trigger_ops st_lsm6dsv16bx_trigger_ops = { + .set_trigger_state = &st_lsm6dsv16bx_trig_set_state, +}; + +static int st_lsm6dsv16bx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsv16bx_event_sensor_set_enable(iio_priv(iio_dev), true); +} + +static int st_lsm6dsv16bx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsv16bx_event_sensor_set_enable(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsv16bx_buffer_ops = { + .preenable = st_lsm6dsv16bx_buffer_preenable, +#if KERNEL_VERSION(5, 10, 0) > LINUX_VERSION_CODE + .postenable = iio_triggered_buffer_postenable, + .predisable = iio_triggered_buffer_predisable, +#endif /* LINUX_VERSION_CODE */ + .postdisable = st_lsm6dsv16bx_buffer_postdisable, +}; + +static int +st_lsm6dsv16bx_config_default_events(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + /* set default wake-up thershold to 93750 ug */ + err = st_lsm6dsv16bx_set_wake_up_thershold(hw, 93750); + if (err < 0) + return err; + + /* set default wake-up duration to 0 */ + err = st_lsm6dsv16bx_set_wake_up_duration(hw, 0); + if (err < 0) + return err; + + /* set default FF threshold to 312 mg */ + err = st_lsm6dsv16bx_set_freefall_threshold(hw, 312); + if (err < 0) + return err; + + /* set default 6D threshold to 60 degrees */ + err = st_lsm6dsv16bx_set_6D_threshold(hw, 60); + if (err < 0) + return err; + + return st_lsm6dsv16bx_init_tap(hw); +} + +int st_lsm6dsv16bx_probe_event(struct st_lsm6dsv16bx_hw *hw) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + irqreturn_t (*pthread[ST_LSM6DSV16BX_ID_MAX - ST_LSM6DSV16BX_ID_WK])(int irq, void *p) = { + [0] = st_lsm6dsv16bx_wk_handler_thread, + [1] = st_lsm6dsv16bx_6D_handler_thread, + /* add here all other trigger handler funcions */ + }; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_event_sensor_list); + i++) { + enum st_lsm6dsv16bx_sensor_id id = + st_lsm6dsv16bx_event_sensor_list[i]; + + hw->iio_devs[id] = st_lsm6dsv16bx_alloc_event_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + /* configure trigger sensors */ + for (i = 0; + i < ARRAY_SIZE(st_lsm6dsv16bx_event_trigger_sensor_list); + i++) { + enum st_lsm6dsv16bx_sensor_id id = + st_lsm6dsv16bx_event_trigger_sensor_list[i]; + iio_dev = hw->iio_devs[id]; + sensor = iio_priv(iio_dev); + + err = devm_iio_triggered_buffer_setup(hw->dev, iio_dev, + NULL, pthread[id - ST_LSM6DSV16BX_ID_WK], + &st_lsm6dsv16bx_buffer_ops); + if (err < 0) + return err; + + sensor->trig = devm_iio_trigger_alloc(hw->dev, + "%s-trigger", + iio_dev->name); + if (!sensor->trig) { + dev_err(hw->dev, + "failed to allocate iio trigger.\n"); + + return -ENOMEM; + } + + iio_trigger_set_drvdata(sensor->trig, iio_dev); + sensor->trig->ops = &st_lsm6dsv16bx_trigger_ops; + sensor->trig->dev.parent = hw->dev; + + err = devm_iio_trigger_register(hw->dev, sensor->trig); + if (err < 0) { + dev_err(hw->dev, + "failed to register iio trigger.\n"); + + return err; + } + + iio_dev->trig = iio_trigger_get(sensor->trig); + } + + return st_lsm6dsv16bx_config_default_events(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_hwtimestamp.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_hwtimestamp.c new file mode 100644 index 000000000000..f92d8fefc327 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_hwtimestamp.c @@ -0,0 +1,116 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx hwtimestamp library driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +#define ST_LSM6DSV16BX_TSYNC_OFFSET_NS (300 * 1000LL) + +static void st_lsm6dsv16bx_read_hw_timestamp(struct st_lsm6dsv16bx_hw *hw) +{ + s64 timestamp_hw_global; + s64 eventLSB, eventMSB; + __le32 timestamp_hw; + s64 timestamp_cpu; + __le32 tmp; + int err; + + err = st_lsm6dsv16bx_read_locked(hw, ST_LSM6DSV16BX_REG_TIMESTAMP0_ADDR, + (u8 *)×tamp_hw, + sizeof(timestamp_hw)); + if (err < 0) + return; + + timestamp_cpu = iio_get_time_ns(hw->iio_devs[0]) - + ST_LSM6DSV16BX_TSYNC_OFFSET_NS; + + eventLSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 0, + STM_IIO_EV_TYPE_TIME_SYNC, 0, 0, 0); + eventMSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 1, + STM_IIO_EV_TYPE_TIME_SYNC, 0, 0, 0); + + spin_lock_irq(&hw->hwtimestamp_lock); + timestamp_hw_global = (hw->hw_timestamp_global & GENMASK_ULL(63, 32)) | + (u32)le32_to_cpu(timestamp_hw); + spin_unlock_irq(&hw->hwtimestamp_lock); + + tmp = cpu_to_le32((u32)timestamp_hw_global); + memcpy(&((int8_t *)&eventLSB)[0], &tmp, sizeof(tmp)); + + tmp = cpu_to_le32((u32)(timestamp_hw_global >> 32)); + memcpy(&((int8_t *)&eventMSB)[0], &tmp, sizeof(tmp)); + + if (hw->enable_mask & BIT_ULL(ST_LSM6DSV16BX_ID_GYRO)) { + iio_push_event(hw->iio_devs[ST_LSM6DSV16BX_ID_GYRO], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSV16BX_ID_GYRO], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSV16BX_ID_ACC)) { + iio_push_event(hw->iio_devs[ST_LSM6DSV16BX_ID_ACC], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSV16BX_ID_ACC], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSV16BX_ID_TEMP)) { + iio_push_event(hw->iio_devs[ST_LSM6DSV16BX_ID_TEMP], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSV16BX_ID_TEMP], eventMSB, + timestamp_cpu); + } + + if (hw->timesync_c < 6) + hw->timesync_c++; + else + hw->timesync_ktime = ktime_set(0, ST_LSM6DSV16BX_DEFAULT_KTIME); +} + +static void st_lsm6dsv16bx_timesync_fn(struct work_struct *work) +{ + struct st_lsm6dsv16bx_hw *hw = container_of(work, struct st_lsm6dsv16bx_hw, + timesync_work); + + st_lsm6dsv16bx_read_hw_timestamp(hw); +} + +static enum hrtimer_restart st_lsm6dsv16bx_timer_fn(struct hrtimer *timer) +{ + struct st_lsm6dsv16bx_hw *hw; + + hw = container_of(timer, struct st_lsm6dsv16bx_hw, timesync_timer); + hrtimer_forward(timer, hrtimer_cb_get_time(timer), hw->timesync_ktime); + queue_work(hw->timesync_workqueue, &hw->timesync_work); + + return HRTIMER_RESTART; +} + +int st_lsm6dsv16bx_hwtimesync_init(struct st_lsm6dsv16bx_hw *hw) +{ + hw->timesync_c = 0; + hw->timesync_ktime = ktime_set(0, ST_LSM6DSV16BX_DEFAULT_KTIME); + hrtimer_init(&hw->timesync_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + hw->timesync_timer.function = st_lsm6dsv16bx_timer_fn; + + spin_lock_init(&hw->hwtimestamp_lock); + hw->hw_timestamp_global = 0; + + hw->timesync_workqueue = create_singlethread_workqueue("st_lsm6dsv16bx_workqueue"); + if (!hw->timesync_workqueue) + return -ENOMEM; + + INIT_WORK(&hw->timesync_work, st_lsm6dsv16bx_timesync_fn); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsv16bx_hwtimesync_init); diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i2c.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i2c.c new file mode 100644 index 000000000000..35bda8a026ff --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i2c.c @@ -0,0 +1,69 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +static const struct regmap_config st_lsm6dsv16bx_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsv16bx_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, + &st_lsm6dsv16bx_i2c_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, + "Failed to register i2c regmap %d\n", + (int)PTR_ERR(regmap)); + + return PTR_ERR(regmap); + } + + return st_lsm6dsv16bx_probe(&client->dev, client->irq, hw_id, regmap); +} + +static const struct of_device_id st_lsm6dsv16bx_i2c_of_match[] = { + { + .compatible = "st," ST_LSM6DSV16BX_DEV_NAME, + .data = (void *)ST_LSM6DSV16BX_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsv16bx_i2c_of_match); + +static const struct i2c_device_id st_lsm6dsv16bx_i2c_id_table[] = { + { ST_LSM6DSV16BX_DEV_NAME, ST_LSM6DSV16BX_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsv16bx_i2c_id_table); + +static struct i2c_driver st_lsm6dsv16bx_driver = { + .driver = { + .name = "st_lsm6dsv16bx_i2c", + .pm = &st_lsm6dsv16bx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsv16bx_i2c_of_match), + }, + .probe = st_lsm6dsv16bx_i2c_probe, + .id_table = st_lsm6dsv16bx_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsv16bx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsv16bx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i3c.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i3c.c new file mode 100644 index 000000000000..7c1a27da45a3 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_i3c.c @@ -0,0 +1,60 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx i3c driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +static const struct i3c_device_id st_lsm6dsv16bx_i3c_ids[] = { + I3C_DEVICE(0x0104, ST_LSM6DSV16BX_WHOAMI_VAL, (void *)ST_LSM6DSV16BX_ID), + {}, +}; +MODULE_DEVICE_TABLE(i3c, st_lsm6dsv16bx_i3c_ids); + +static int st_lsm6dsv16bx_i3c_probe(struct i3c_device *i3cdev) +{ + struct regmap_config st_lsm6dsv16bx_i3c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + }; + const struct i3c_device_id *id = + i3c_device_match_id(i3cdev, st_lsm6dsv16bx_i3c_ids); + struct regmap *regmap; + + regmap = devm_regmap_init_i3c(i3cdev, + &st_lsm6dsv16bx_i3c_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&i3cdev->dev, + "Failed to register i3c regmap %d\n", + (int)PTR_ERR(regmap)); + + return PTR_ERR(regmap); + } + + return st_lsm6dsv16bx_probe(&i3cdev->dev, 0, + (uintptr_t)id->data, regmap); +} + +static struct i3c_driver st_lsm6dsv16bx_driver = { + .driver = { + .name = "st_lsm6dsv16bx_i3c", + .pm = &st_lsm6dsv16bx_pm_ops, + }, + .probe = st_lsm6dsv16bx_i3c_probe, + .id_table = st_lsm6dsv16bx_i3c_ids, +}; +module_i3c_driver(st_lsm6dsv16bx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsv16bx i3c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_mlc.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_mlc.c new file mode 100644 index 000000000000..b6ab2bfc56a4 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_mlc.c @@ -0,0 +1,847 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx machine learning core driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsv16bx.h" + +#define ST_LSM6DSV16BX_MLC_LOADER_VERSION "0.3" + +/* number of machine learning core available on device hardware */ +#define ST_LSM6DSV16BX_MLC_MAX_NUMBER 4 +#define ST_LSM6DSV16BX_FSM_MAX_NUMBER 8 + +#ifdef CONFIG_IIO_LSM6DSV16BX_MLC_BUILTIN_FIRMWARE +static const u8 st_lsm6dsv16bx_mlc_fw[] = { + #include "st_lsm6dsv16bx_mlc.fw" +}; +DECLARE_BUILTIN_FIRMWARE(LSM6DSV16BX_MLC_FIRMWARE_NAME, st_lsm6dsv16bx_mlc_fw); +#else /* CONFIG_IIO_LSM6DSV16BX_MLC_BUILTIN_FIRMWARE */ +#define LSM6DSV16BX_MLC_FIRMWARE_NAME "st_lsm6dsv16bx_mlc.bin" +#endif /* CONFIG_IIO_LSM6DSV16BX_MLC_BUILTIN_FIRMWARE */ + +#ifdef CONFIG_IIO_ST_LSM6DSV16BX_MLC_PRELOAD +#include "st_lsm6dsv16bx_preload_mlc.h" +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_MLC_PRELOAD */ + + +/* converts MLC odr to main sensor trigger odr (acc) */ +static const uint16_t mlc_odr_data[] = { + [0x00] = 15, + [0x01] = 30, + [0x02] = 50, + [0x03] = 120, + [0x04] = 240, + [0x05] = 480, + [0x06] = 960, +}; + +static const uint16_t fsm_odr_data[] = { + [0x00] = 15, + [0x01] = 30, + [0x02] = 50, + [0x03] = 120, + [0x04] = 240, + [0x05] = 480, + [0x06] = 960, +}; + +static struct iio_dev * +st_lsm6dsv16bx_mlc_alloc_iio_dev(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id); + +static const unsigned long st_lsm6dsv16bx_mlc_available_scan_masks[] = { + 0x1, 0x0 +}; + +static inline int +st_lsm6dsv16bx_read_page_locked(struct st_lsm6dsv16bx_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + err = regmap_bulk_read(hw->regmap, addr, val, len); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsv16bx_write_page_locked(struct st_lsm6dsv16bx_hw *hw, unsigned int addr, + unsigned int *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + err = regmap_bulk_write(hw->regmap, addr, val, len); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsv16bx_update_page_bits_locked(struct st_lsm6dsv16bx_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 1); + err = regmap_update_bits(hw->regmap, addr, mask, val); + st_lsm6dsv16bx_set_page_access(hw, + ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK, + 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static int +st_lsm6dsv16bx_mlc_enable_sensor(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int i, id, err = 0; + + /* enable acc sensor as trigger */ + err = st_lsm6dsv16bx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + if (sensor->status == ST_LSM6DSV16BX_MLC_ENABLED) { + int value; + + value = enable ? hw->mlc_config->mlc_int_mask : 0; + err = st_lsm6dsv16bx_write_page_locked(hw, + hw->mlc_config->mlc_int_addr, + &value, 1); + if (err < 0) + return err; + + /* + * enable mlc core + * only one mlc so not need to check if other running + */ + err = st_lsm6dsv16bx_update_page_bits_locked(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSV16BX_MLC_EN_MASK, + ST_LSM6DSV16BX_SHIFT_VAL(enable, + ST_LSM6DSV16BX_MLC_EN_MASK)); + if (err < 0) + return err; + + dev_info(sensor->hw->dev, + "Enabling MLC sensor %d to %d (INT %x)\n", + sensor->id, enable, value); + } else if (sensor->status == ST_LSM6DSV16BX_FSM_ENABLED) { + int value; + + value = enable ? hw->mlc_config->fsm_int_mask : 0; + err = st_lsm6dsv16bx_write_page_locked(hw, + hw->mlc_config->fsm_int_addr, + &value, 1); + if (err < 0) + return err; + + /* enable fsm core */ + for (i = 0; i < ST_LSM6DSV16BX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsv16bx_fsm_sensor_list[i]; + if (hw->enable_mask & BIT(id)) + break; + } + + /* check for any other fsm already enabled */ + if (enable || i == ST_LSM6DSV16BX_FSM_MAX_NUMBER) { + err = st_lsm6dsv16bx_update_page_bits_locked(hw, + ST_LSM6DSV16BX_REG_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSV16BX_FSM_EN_MASK, + ST_LSM6DSV16BX_SHIFT_VAL(enable, + ST_LSM6DSV16BX_FSM_EN_MASK)); + if (err < 0) + return err; + } + + dev_info(sensor->hw->dev, + "Enabling FSM sensor %d to %d (INT %x)\n", + sensor->id, enable, value); + } else { + dev_err(hw->dev, "invalid sensor configuration\n"); + err = -ENODEV; + + return err; + } + + if (enable) + hw->enable_mask |= BIT(sensor->id); + else + hw->enable_mask &= ~BIT(sensor->id); + + return err < 0 ? err : 0; +} + +static int +st_lsm6dsv16bx_mlc_write_event_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, int state) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsv16bx_mlc_enable_sensor(sensor, state); +} + +static int +st_lsm6dsv16bx_mlc_read_event_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/* + * st_lsm6dsv16bx_verify_mlc_fsm_support - Verify device supports MLC/FSM + * + * Before to load a MLC/FSM configuration check the MLC/FSM HW block + * available for this hw device id. + */ +static int st_lsm6dsv16bx_verify_mlc_fsm_support(const struct firmware *fw, + struct st_lsm6dsv16bx_hw *hw) +{ + bool stmc_page = false; + u8 reg, val; + int i = 0; + + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == ST_LSM6DSV16BX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = true; + } else if (reg == ST_LSM6DSV16BX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ~ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSV16BX_REG_MLC_INT1_ADDR: + case ST_LSM6DSV16BX_REG_MLC_INT2_ADDR: + if (!hw->settings->st_mlc_probe) + return -ENODEV; + break; + case ST_LSM6DSV16BX_REG_FSM_INT1_ADDR: + case ST_LSM6DSV16BX_REG_FSM_INT2_ADDR: + if (!hw->settings->st_fsm_probe) + return -ENODEV; + break; + default: + break; + } + } + } + + return 0; +} + +/* parse and program mlc fragments */ +static int st_lsm6dsv16bx_program_mlc(const struct firmware *fw, + struct st_lsm6dsv16bx_hw *hw) +{ + u8 mlc_int = 0, mlc_num = 0, fsm_num = 0, skip = 0; + u8 fsm_int = 0, reg, val, req_odr = 0; + bool stmc_page = false; + int ret, i = 0; + + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == ST_LSM6DSV16BX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = true; + } else if (reg == ST_LSM6DSV16BX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ~ST_LSM6DSV16BX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSV16BX_REG_MLC_INT1_ADDR: + case ST_LSM6DSV16BX_REG_MLC_INT2_ADDR: + mlc_int |= val; + mlc_num++; + skip = 1; + break; + case ST_LSM6DSV16BX_REG_FSM_INT1_ADDR: + case ST_LSM6DSV16BX_REG_FSM_INT2_ADDR: + fsm_int |= val; + fsm_num++; + skip = 1; + break; + case ST_LSM6DSV16BX_REG_EMB_FUNC_EN_B_ADDR: + skip = 1; + break; + default: + break; + } + } else if (reg == ST_LSM6DSV16BX_REG_CTRL1_ADDR) { + /* save required xl odr and skip write to reg */ + req_odr = max_t(u8, req_odr, (val & GENMASK(3, 0))); + skip = 1; + } + + if (!skip) { + ret = regmap_write(hw->regmap, reg, val); + if (ret) { + dev_err(hw->dev, + "regmap_write fails\n"); + + return ret; + } + } + + skip = 0; + + if (mlc_num >= ST_LSM6DSV16BX_MLC_MAX_NUMBER || + fsm_num >= ST_LSM6DSV16BX_FSM_MAX_NUMBER) + break; + } + + hw->mlc_config->bin_len = fw->size; + + if (mlc_num) { + hw->mlc_config->mlc_int_mask = mlc_int; + hw->mlc_config->mlc_int_addr = (hw->int_pin == 1 ? + ST_LSM6DSV16BX_REG_MLC_INT1_ADDR : + ST_LSM6DSV16BX_REG_MLC_INT2_ADDR); + + hw->mlc_config->status |= ST_LSM6DSV16BX_MLC_ENABLED; + hw->mlc_config->mlc_configured += mlc_num; + hw->mlc_config->requested_odr = mlc_odr_data[req_odr]; + } + + if (fsm_num) { + hw->mlc_config->fsm_int_mask = fsm_int; + hw->mlc_config->fsm_int_addr = (hw->int_pin == 1 ? + ST_LSM6DSV16BX_REG_FSM_INT1_ADDR : + ST_LSM6DSV16BX_REG_FSM_INT2_ADDR); + + hw->mlc_config->status |= ST_LSM6DSV16BX_FSM_ENABLED; + hw->mlc_config->fsm_configured += fsm_num; + hw->mlc_config->requested_odr = fsm_odr_data[req_odr]; + } + + return fsm_num + mlc_num; +} + +static void st_lsm6dsv16bx_mlc_update(const struct firmware *fw, void *context) +{ + struct st_lsm6dsv16bx_hw *hw = context; + enum st_lsm6dsv16bx_sensor_id id; + int ret, i; + + if (!fw) { + dev_err(hw->dev, "could not get binary firmware\n"); + + return; + } + + ret = st_lsm6dsv16bx_verify_mlc_fsm_support(fw, hw); + if (ret) { + dev_err(hw->dev, "invalid file format for device\n"); + + return; + } + + ret = st_lsm6dsv16bx_program_mlc(fw, hw); + if (ret > 0) { + u8 fsm_mask = hw->mlc_config->fsm_int_mask; + u8 mlc_mask = hw->mlc_config->mlc_int_mask; + + dev_info(hw->dev, + "MLC loaded (%d) MLC %01x FSM %02x\n", + ret, mlc_mask, fsm_mask); + + for (i = 0; i < ST_LSM6DSV16BX_MLC_MAX_NUMBER; i++) { + if (mlc_mask & BIT(i)) { + id = st_lsm6dsv16bx_mlc_sensor_list[i]; + hw->iio_devs[id] = + st_lsm6dsv16bx_mlc_alloc_iio_dev(hw, id); + if (!hw->iio_devs[id]) + goto release; + + ret = iio_device_register(hw->iio_devs[id]); + if (ret) + goto release; + } + } + + for (i = 0; i < ST_LSM6DSV16BX_FSM_MAX_NUMBER; i++) { + if (fsm_mask & BIT(i)) { + id = st_lsm6dsv16bx_fsm_sensor_list[i]; + hw->iio_devs[id] = + st_lsm6dsv16bx_mlc_alloc_iio_dev(hw, id); + if (!hw->iio_devs[id]) + goto release; + + ret = iio_device_register(hw->iio_devs[id]); + if (ret) + goto release; + } + } + } + +release: + /* + * internal firmware don't release it because stored in + * const segment + */ + if (hw->preload_mlc) { + hw->preload_mlc = 0; + + return; + } + + release_firmware(fw); +} + +static int st_lsm6dsv16bx_mlc_flush_single(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id) +{ + struct st_lsm6dsv16bx_sensor *sensor_mlc; + struct iio_dev *iio_dev; + int ret; + + iio_dev = hw->iio_devs[id]; + if (!iio_dev) + return -ENODEV; + + sensor_mlc = iio_priv(iio_dev); + ret = st_lsm6dsv16bx_mlc_enable_sensor(sensor_mlc, false); + if (ret < 0) + return ret; + + iio_device_unregister(iio_dev); + kfree(iio_dev->channels); + iio_device_free(iio_dev); + hw->iio_devs[id] = NULL; + + return 0; +} + +static int st_lsm6dsv16bx_mlc_flush_all(struct st_lsm6dsv16bx_hw *hw) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_mlc_sensor_list); i++) + st_lsm6dsv16bx_mlc_flush_single(hw, st_lsm6dsv16bx_mlc_sensor_list[i]); + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsv16bx_fsm_sensor_list); i++) + st_lsm6dsv16bx_mlc_flush_single(hw, st_lsm6dsv16bx_fsm_sensor_list[i]); + + return 0; +} + +static ssize_t st_lsm6dsv16bx_mlc_info(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "mlc %02x fsm %02x\n", + hw->mlc_config->mlc_configured, + hw->mlc_config->fsm_configured); +} + +static ssize_t +st_lsm6dsv16bx_mlc_get_version(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "mlc loader Version %s\n", + ST_LSM6DSV16BX_MLC_LOADER_VERSION); +} + +static ssize_t st_lsm6dsv16bx_mlc_flush(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + int ret; + + ret = st_lsm6dsv16bx_mlc_flush_all(hw); + memset(hw->mlc_config, 0, sizeof(*hw->mlc_config)); + + return ret < 0 ? ret : size; +} + +static ssize_t +st_lsm6dsv16bx_mlc_upload_firmware(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err; + + err = request_firmware_nowait(THIS_MODULE, true, + LSM6DSV16BX_MLC_FIRMWARE_NAME, + dev, GFP_KERNEL, + sensor->hw, + st_lsm6dsv16bx_mlc_update); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6dsv16bx_mlc_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsv16bx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%d\n", + hw->mlc_config->requested_odr); +} + +static IIO_DEVICE_ATTR(config_info, 0444, st_lsm6dsv16bx_mlc_info, NULL, 0); +static IIO_DEVICE_ATTR(flush_config, 0200, NULL, st_lsm6dsv16bx_mlc_flush, 0); +static IIO_DEVICE_ATTR(loader_version, 0444, + st_lsm6dsv16bx_mlc_get_version, NULL, 0); +static IIO_DEVICE_ATTR(load_mlc, 0200, + NULL, st_lsm6dsv16bx_mlc_upload_firmware, 0); +static IIO_DEVICE_ATTR(odr, 0444, st_lsm6dsv16bx_mlc_odr, NULL, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsv16bx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsv16bx_mlc_event_attributes[] = { + &iio_dev_attr_config_info.dev_attr.attr, + &iio_dev_attr_loader_version.dev_attr.attr, + &iio_dev_attr_load_mlc.dev_attr.attr, + &iio_dev_attr_flush_config.dev_attr.attr, + &iio_dev_attr_odr.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group +st_lsm6dsv16bx_mlc_event_attribute_group = { + .attrs = st_lsm6dsv16bx_mlc_event_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_mlc_event_info = { + .attrs = &st_lsm6dsv16bx_mlc_event_attribute_group, + .read_event_config = st_lsm6dsv16bx_mlc_read_event_config, + .write_event_config = st_lsm6dsv16bx_mlc_write_event_config, +}; + +static ssize_t st_lsm6dsv16bx_mlc_x_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsv16bx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return scnprintf(buf, PAGE_SIZE, "%d.%02d\n", + sensor->odr, sensor->uodr); +} + +static IIO_DEVICE_ATTR(odr_x, 0444, st_lsm6dsv16bx_mlc_x_odr, NULL, 0); + +static struct attribute *st_lsm6dsv16bx_mlc_x_event_attributes[] = { + &iio_dev_attr_odr_x.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group +st_lsm6dsv16bx_mlc_x_event_attribute_group = { + .attrs = st_lsm6dsv16bx_mlc_x_event_attributes, +}; +static const struct iio_info st_lsm6dsv16bx_mlc_x_event_info = { + .attrs = &st_lsm6dsv16bx_mlc_x_event_attribute_group, + .read_event_config = st_lsm6dsv16bx_mlc_read_event_config, + .write_event_config = st_lsm6dsv16bx_mlc_write_event_config, +}; + +static struct iio_dev * +st_lsm6dsv16bx_mlc_alloc_iio_dev(struct st_lsm6dsv16bx_hw *hw, + enum st_lsm6dsv16bx_sensor_id id) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_chan_spec *channels; + struct iio_dev *iio_dev; + + /* devm management only for ST_LSM6DSV16BX_ID_MLC */ + if (id == ST_LSM6DSV16BX_ID_MLC) { + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + } else { +#if KERNEL_VERSION(5, 9, 0) <= LINUX_VERSION_CODE + iio_dev = iio_device_alloc(NULL, sizeof(*sensor)); +#else /* LINUX_VERSION_CODE */ + iio_dev = iio_device_alloc(sizeof(*sensor)); +#endif /* LINUX_VERSION_CODE */ + } + + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = hw->dev; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + + switch (id) { + case ST_LSM6DSV16BX_ID_MLC: { + const struct iio_chan_spec st_lsm6dsv16bx_mlc_channels[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = devm_kzalloc(hw->dev, + sizeof(st_lsm6dsv16bx_mlc_channels), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsv16bx_mlc_channels, + sizeof(st_lsm6dsv16bx_mlc_channels)); + + iio_dev->available_scan_masks = + st_lsm6dsv16bx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_mlc_channels); + iio_dev->info = &st_lsm6dsv16bx_mlc_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_loader", hw->settings->id.name); + break; + } + case ST_LSM6DSV16BX_ID_MLC_0: + case ST_LSM6DSV16BX_ID_MLC_1: + case ST_LSM6DSV16BX_ID_MLC_2: + case ST_LSM6DSV16BX_ID_MLC_3: { + const struct iio_chan_spec st_lsm6dsv16bx_mlc_x_ch[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsv16bx_mlc_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsv16bx_mlc_x_ch, + sizeof(st_lsm6dsv16bx_mlc_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsv16bx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_mlc_x_ch); + iio_dev->info = &st_lsm6dsv16bx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc_%d", hw->settings->id.name, + id - ST_LSM6DSV16BX_ID_MLC_0); + sensor->outreg_addr = ST_LSM6DSV16BX_REG_MLC1_SRC_ADDR + + id - ST_LSM6DSV16BX_ID_MLC_0; + sensor->status = ST_LSM6DSV16BX_MLC_ENABLED; + sensor->odr = hw->mlc_config->requested_odr; + sensor->uodr = 0; + + break; + } + case ST_LSM6DSV16BX_ID_FSM_0: + case ST_LSM6DSV16BX_ID_FSM_1: + case ST_LSM6DSV16BX_ID_FSM_2: + case ST_LSM6DSV16BX_ID_FSM_3: + case ST_LSM6DSV16BX_ID_FSM_4: + case ST_LSM6DSV16BX_ID_FSM_5: + case ST_LSM6DSV16BX_ID_FSM_6: + case ST_LSM6DSV16BX_ID_FSM_7: { + const struct iio_chan_spec st_lsm6dsv16bx_fsm_x_ch[] = { + ST_LSM6DSV16BX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsv16bx_fsm_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsv16bx_fsm_x_ch, + sizeof(st_lsm6dsv16bx_fsm_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsv16bx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_fsm_x_ch); + iio_dev->info = &st_lsm6dsv16bx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_fsm_%d", hw->settings->id.name, + id - ST_LSM6DSV16BX_ID_FSM_0); + sensor->outreg_addr = ST_LSM6DSV16BX_REG_FSM_OUTS1_ADDR + + id - ST_LSM6DSV16BX_ID_FSM_0; + sensor->status = ST_LSM6DSV16BX_FSM_ENABLED; + sensor->odr = hw->mlc_config->requested_odr; + sensor->uodr = 0; + break; + } + default: + dev_err(hw->dev, "invalid sensor id %d\n", id); + + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +int st_lsm6dsv16bx_mlc_check_status(struct st_lsm6dsv16bx_hw *hw) +{ + struct st_lsm6dsv16bx_sensor *sensor; + u8 i, mlc_status, id, event[8]; + struct iio_dev *iio_dev; + u8 fsm_status; + int err = 0; + + if (hw->mlc_config->status & ST_LSM6DSV16BX_MLC_ENABLED) { + err = st_lsm6dsv16bx_read_locked(hw, + ST_LSM6DSV16BX_REG_MLC_STATUS_MAINPAGE_ADDR, + (void *)&mlc_status, 1); + if (err) + return err; + + if (mlc_status) { + for (i = 0; i < ST_LSM6DSV16BX_MLC_MAX_NUMBER; i++) { + id = st_lsm6dsv16bx_mlc_sensor_list[i]; + if (!(hw->enable_mask & BIT(id))) + continue; + + if (mlc_status & BIT(i)) { + iio_dev = hw->iio_devs[id]; + if (!iio_dev) { + err = -ENOENT; + + return err; + } + + sensor = iio_priv(iio_dev); + err = st_lsm6dsv16bx_read_page_locked(hw, + sensor->outreg_addr, + (void *)&event[i], 1); + if (err) + return err; + + iio_push_event(iio_dev, (u64)event[i], + iio_get_time_ns(iio_dev)); + + dev_info(hw->dev, + "MLC %d Status %x MLC EVENT %llx\n", + id, mlc_status, (u64)event[i]); + } + } + } + } + + if (hw->mlc_config->status & ST_LSM6DSV16BX_FSM_ENABLED) { + err = st_lsm6dsv16bx_read_locked(hw, + ST_LSM6DSV16BX_REG_FSM_STATUS_MAINPAGE_ADDR, + (void *)&fsm_status, 1); + if (err) + return err; + + if (fsm_status) { + for (i = 0; i < ST_LSM6DSV16BX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsv16bx_fsm_sensor_list[i]; + if (!(hw->enable_mask & BIT(id))) + continue; + + if (fsm_status & BIT(i)) { + iio_dev = hw->iio_devs[id]; + if (!iio_dev) { + err = -ENOENT; + + return err; + } + + sensor = iio_priv(iio_dev); + err = st_lsm6dsv16bx_read_page_locked(hw, + sensor->outreg_addr, + (void *)&event[i], 1); + if (err) + return err; + + iio_push_event(iio_dev, (u64)event[i], + iio_get_time_ns(iio_dev)); + + dev_info(hw->dev, + "FSM %d Status %x FSM EVENT %llx\n", + id, mlc_status, (u64)event[i]); + } + } + } + } + + return err; +} + +int st_lsm6dsv16bx_mlc_init_preload(struct st_lsm6dsv16bx_hw *hw) +{ +#ifdef CONFIG_IIO_ST_LSM6DSV16BX_MLC_PRELOAD + hw->preload_mlc = 1; + st_lsm6dsv16bx_mlc_update(&st_lsm6dsv16bx_mlc_preload, hw); +#endif /* CONFIG_IIO_ST_LSM6DSV16BX_MLC_PRELOAD */ + + return 0; +} + +int st_lsm6dsv16bx_mlc_probe(struct st_lsm6dsv16bx_hw *hw) +{ + hw->iio_devs[ST_LSM6DSV16BX_ID_MLC] = + st_lsm6dsv16bx_mlc_alloc_iio_dev(hw, ST_LSM6DSV16BX_ID_MLC); + if (!hw->iio_devs[ST_LSM6DSV16BX_ID_MLC]) + return -ENOMEM; + + hw->mlc_config = devm_kzalloc(hw->dev, + sizeof(struct st_lsm6dsv16bx_mlc_config_t), + GFP_KERNEL); + if (!hw->mlc_config) + return -ENOMEM; + + return 0; +} + +int st_lsm6dsv16bx_mlc_remove(struct device *dev) +{ + struct st_lsm6dsv16bx_hw *hw = dev_get_drvdata(dev); + + return st_lsm6dsv16bx_mlc_flush_all(hw); +} +EXPORT_SYMBOL(st_lsm6dsv16bx_mlc_remove); diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_preload_mlc.h b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_preload_mlc.h new file mode 100644 index 000000000000..025c2b8ba11f --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_preload_mlc.h @@ -0,0 +1,20 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSV16BX_PRELOAD_MLC_H +#define ST_LSM6DSV16BX_PRELOAD_MLC_H + +static const u8 mlcdata[] = { + /* put here MLC/FSM configuration */ +}; + +static struct firmware st_lsm6dsv16bx_mlc_preload = { + .size = sizeof(mlcdata), + .data = mlcdata +}; + +#endif /* ST_LSM6DSV16BX_PRELOAD_MLC_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_qvar.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_qvar.c new file mode 100644 index 000000000000..ad4dddc7e512 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_qvar.c @@ -0,0 +1,191 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx qvar sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +static const struct st_lsm6dsv16bx_odr_table_entry +st_lsm6dsv16bx_qvar_odr_table = { + .size = 1, + .odr_avl[0] = { 240, 0, 0x00, 0x00 }, +}; + +static const struct iio_chan_spec st_lsm6dsv16bx_qvar_channels[] = { + { + .type = IIO_ALTVOLTAGE, + .address = ST_LSM6DSV16BX_REG_OUT_QVAR_ADDR, + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + } + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static int st_lsm6dsv16bx_qvar_init(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + /* enable fifo batching by default */ + err = st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_COUNTER_BDR_REG1_ADDR, + ST_LSM6DSV16BX_AH_QVAR_BATCH_EN_MASK, + 1); + if (err < 0) + return err; + + /* impedance selection 235 Mohm */ + return st_lsm6dsv16bx_write_with_mask(hw, + ST_LSM6DSV16BX_REG_CTRL7_ADDR, + ST_LSM6DSV16BX_AH_QVAR_C_ZIN_MASK, + 3); +} + +static ssize_t +st_lsm6dsv16bx_sysfs_qvar_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int len = 0; + int i; + + for (i = 0; i < st_lsm6dsv16bx_qvar_odr_table.size; i++) { + if (!st_lsm6dsv16bx_qvar_odr_table.odr_avl[i].hz) + continue; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lsm6dsv16bx_qvar_odr_table.odr_avl[i].hz, + st_lsm6dsv16bx_qvar_odr_table.odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsv16bx_sysfs_qvar_sampling_freq_avail); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsv16bx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsv16bx_qvar_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group +st_lsm6dsv16bx_qvar_attribute_group = { + .attrs = st_lsm6dsv16bx_qvar_attributes, +}; + +static const struct iio_info st_lsm6dsv16bx_qvar_info = { + .attrs = &st_lsm6dsv16bx_qvar_attribute_group, +}; + +static const unsigned long st_lsm6dsv16bx_qvar_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int +_st_lsm6dsv16bx_qvar_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + u16 odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsv16bx_sensor_set_enable(sensor, odr); + if (err < 0) + return err; + + err = st_lsm6dsv16bx_write_with_mask(sensor->hw, + ST_LSM6DSV16BX_REG_CTRL7_ADDR, + ST_LSM6DSV16BX_AH_QVARx_EN_MASK, + enable ? 3 : 0); + if (err < 0) + return err; + + return st_lsm6dsv16bx_write_with_mask(sensor->hw, + ST_LSM6DSV16BX_REG_CTRL7_ADDR, + ST_LSM6DSV16BX_AH_QVAR_EN_MASK, + enable ? 1 : 0); +} + +int st_lsm6dsv16bx_qvar_sensor_set_enable(struct st_lsm6dsv16bx_sensor *sensor, + bool enable) +{ + int err; + + err = _st_lsm6dsv16bx_qvar_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + if (enable) + sensor->hw->enable_mask |= BIT(sensor->id); + else + sensor->hw->enable_mask &= ~BIT(sensor->id); + + return 0; +} + +static struct iio_dev * +st_lsm6dsv16bx_alloc_qvar_iiodev(struct st_lsm6dsv16bx_hw *hw) +{ + struct st_lsm6dsv16bx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = hw->dev; + + sensor = iio_priv(iio_dev); + sensor->id = ST_LSM6DSV16BX_ID_QVAR; + sensor->hw = hw; + + iio_dev->channels = st_lsm6dsv16bx_qvar_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsv16bx_qvar_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_qvar", hw->settings->id.name); + iio_dev->info = &st_lsm6dsv16bx_qvar_info; + iio_dev->available_scan_masks = st_lsm6dsv16bx_qvar_available_scan_masks; + iio_dev->name = sensor->name; + + sensor->odr = st_lsm6dsv16bx_qvar_odr_table.odr_avl[0].hz; + sensor->uodr = st_lsm6dsv16bx_qvar_odr_table.odr_avl[0].uhz; + sensor->gain = 1; + sensor->watermark = 1; + + return iio_dev; +} + +int st_lsm6dsv16bx_qvar_probe(struct st_lsm6dsv16bx_hw *hw) +{ + int err; + + hw->iio_devs[ST_LSM6DSV16BX_ID_QVAR] = st_lsm6dsv16bx_alloc_qvar_iiodev(hw); + if (!hw->iio_devs[ST_LSM6DSV16BX_ID_QVAR]) + return -ENOMEM; + + err = st_lsm6dsv16bx_qvar_init(hw); + + return err < 0 ? err : 0; +} diff --git a/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_spi.c b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_spi.c new file mode 100644 index 000000000000..174adee21500 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsv16bx/st_lsm6dsv16bx_spi.c @@ -0,0 +1,67 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsv16bx spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2023 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lsm6dsv16bx.h" + +static const struct regmap_config st_lsm6dsv16bx_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsv16bx_spi_probe(struct spi_device *spi) +{ + const struct spi_device_id *id = spi_get_device_id(spi); + int hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_spi(spi, &st_lsm6dsv16bx_spi_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&spi->dev, "Failed to register spi regmap %d\n", + (int)PTR_ERR(regmap)); + + return PTR_ERR(regmap); + } + + return st_lsm6dsv16bx_probe(&spi->dev, spi->irq, hw_id, regmap); +} + +static const struct of_device_id st_lsm6dsv16bx_spi_of_match[] = { + { + .compatible = "st," ST_LSM6DSV16BX_DEV_NAME, + .data = (void *)ST_LSM6DSV16BX_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsv16bx_spi_of_match); + +static const struct spi_device_id st_lsm6dsv16bx_spi_id_table[] = { + { ST_LSM6DSV16BX_DEV_NAME, ST_LSM6DSV16BX_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsv16bx_spi_id_table); + +static struct spi_driver st_lsm6dsv16bx_driver = { + .driver = { + .name = "st_lsm6dsv16bx_spi", + .pm = &st_lsm6dsv16bx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsv16bx_spi_of_match), + }, + .probe = st_lsm6dsv16bx_spi_probe, + .id_table = st_lsm6dsv16bx_spi_id_table, +}; +module_spi_driver(st_lsm6dsv16bx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsv16bx spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/stm_iio_configs/lsm6dsv16bx_defconfig b/stm_iio_configs/lsm6dsv16bx_defconfig new file mode 100644 index 000000000000..c547417b503d --- /dev/null +++ b/stm_iio_configs/lsm6dsv16bx_defconfig @@ -0,0 +1,6 @@ +CONFIG_IIO_ST_LSM6DSV16BX=m +CONFIG_IIO_ST_LSM6DSV16BX_I2C=m +CONFIG_IIO_ST_LSM6DSV16BX_I3C=m +CONFIG_IIO_ST_LSM6DSV16BX_SPI=m +CONFIG_IIO_ST_LSM6DSV16BX_MLC_PRELOAD=y +CONFIG_IIO_ST_LSM6DSV16BX_ASYNC_HW_TIMESTAMP=y