From 29df72386f264d569397fbe12ab0f192858dff6b Mon Sep 17 00:00:00 2001 From: Mark Featherston Date: Mon, 7 Aug 2023 08:30:56 -0700 Subject: [PATCH] st-mems-linux: Imported STM MEMS IIO drivers --- .../bindings/iio/stm/accel/st_acc33.txt | 46 + .../bindings/iio/stm/accel/st_ism303dac.txt | 37 + .../bindings/iio/stm/accel/st_lis2ds12.txt | 39 + .../bindings/iio/stm/accel/st_lis2duxs12.txt | 49 + .../bindings/iio/stm/accel/st_lis2dw12.txt | 36 + .../bindings/iio/stm/accel/st_lis2hh12.txt | 34 + .../bindings/iio/stm/accel/st_lis3dhh.txt | 30 + .../bindings/iio/stm/imu/st_asm330lhhx.txt | 79 + .../bindings/iio/stm/imu/st_imu68.txt | 37 + .../bindings/iio/stm/imu/st_ism330dhcx.txt | 46 + .../bindings/iio/stm/imu/st_ism330dlc.txt | 40 + .../bindings/iio/stm/imu/st_lsm6ds3.txt | 37 + .../bindings/iio/stm/imu/st_lsm6ds3h.txt | 36 + .../bindings/iio/stm/imu/st_lsm6dsm.txt | 41 + .../bindings/iio/stm/imu/st_lsm6dso16is.txt | 55 + .../bindings/iio/stm/imu/st_lsm6dsox.txt | 64 + .../bindings/iio/stm/imu/st_lsm6dsrx.txt | 62 + .../bindings/iio/stm/imu/st_lsm6dsvx.txt | 85 + .../iio/stm/magnetometer/st_mag3d.txt | 41 + .../iio/stm/magnetometer/st_mag40.txt | 43 + .../bindings/iio/stm/pressure/st_lps22df.txt | 44 + .../bindings/iio/stm/pressure/st_lps22hb.txt | 31 + .../bindings/iio/stm/pressure/st_lps22hh.txt | 33 + .../bindings/iio/stm/pressure/st_lps33hw.txt | 31 + .../bindings/iio/stm/st_lis2hh12.txt | 34 + .../bindings/iio/stm/temperature/hts221.txt | 30 + .../bindings/iio/stm/temperature/stts22h.txt | 12 + .../bindings/iio/stm/tmos/sths34pf80.txt | 47 + README.md | 84 + drivers/iio/stm/Kconfig | 20 + drivers/iio/stm/Makefile | 12 + drivers/iio/stm/accel/Kconfig | 192 + drivers/iio/stm/accel/Makefile | 44 + drivers/iio/stm/accel/st_acc33.h | 88 + drivers/iio/stm/accel/st_acc33_buffer.c | 290 ++ drivers/iio/stm/accel/st_acc33_core.c | 533 +++ drivers/iio/stm/accel/st_acc33_i2c.c | 122 + drivers/iio/stm/accel/st_acc33_spi.c | 129 + drivers/iio/stm/accel/st_ism303dac_accel.h | 301 ++ .../iio/stm/accel/st_ism303dac_accel_buffer.c | 208 + .../iio/stm/accel/st_ism303dac_accel_core.c | 1252 ++++++ .../iio/stm/accel/st_ism303dac_accel_i2c.c | 174 + .../iio/stm/accel/st_ism303dac_accel_spi.c | 192 + .../stm/accel/st_ism303dac_accel_trigger.c | 166 + drivers/iio/stm/accel/st_lis2ds12.h | 336 ++ drivers/iio/stm/accel/st_lis2ds12_buffer.c | 236 ++ drivers/iio/stm/accel/st_lis2ds12_core.c | 1528 +++++++ drivers/iio/stm/accel/st_lis2ds12_i2c.c | 181 + drivers/iio/stm/accel/st_lis2ds12_spi.c | 196 + drivers/iio/stm/accel/st_lis2ds12_trigger.c | 189 + drivers/iio/stm/accel/st_lis2duxs12.h | 907 +++++ .../iio/stm/accel/st_lis2duxs12_basicfunc.c | 1065 +++++ drivers/iio/stm/accel/st_lis2duxs12_buffer.c | 751 ++++ drivers/iio/stm/accel/st_lis2duxs12_core.c | 1705 ++++++++ drivers/iio/stm/accel/st_lis2duxs12_embfunc.c | 205 + drivers/iio/stm/accel/st_lis2duxs12_i2c.c | 80 + drivers/iio/stm/accel/st_lis2duxs12_i3c.c | 62 + drivers/iio/stm/accel/st_lis2duxs12_mlc.c | 843 ++++ drivers/iio/stm/accel/st_lis2duxs12_qvar.c | 264 ++ drivers/iio/stm/accel/st_lis2duxs12_spi.c | 80 + drivers/iio/stm/accel/st_lis2dw12.h | 103 + drivers/iio/stm/accel/st_lis2dw12_buffer.c | 328 ++ drivers/iio/stm/accel/st_lis2dw12_core.c | 1048 +++++ drivers/iio/stm/accel/st_lis2dw12_i2c.c | 103 + drivers/iio/stm/accel/st_lis2dw12_spi.c | 111 + drivers/iio/stm/accel/st_lis2hh12.h | 214 + drivers/iio/stm/accel/st_lis2hh12_buffer.c | 169 + drivers/iio/stm/accel/st_lis2hh12_core.c | 899 +++++ drivers/iio/stm/accel/st_lis2hh12_i2c.c | 157 + drivers/iio/stm/accel/st_lis2hh12_spi.c | 184 + drivers/iio/stm/accel/st_lis2hh12_trigger.c | 110 + drivers/iio/stm/accel/st_lis3dhh.c | 430 ++ drivers/iio/stm/accel/st_lis3dhh.h | 62 + drivers/iio/stm/accel/st_lis3dhh_buffer.c | 282 ++ drivers/iio/stm/imu/Kconfig | 20 + drivers/iio/stm/imu/Makefile | 16 + drivers/iio/stm/imu/st_asm330lhhx/Kconfig | 45 + drivers/iio/stm/imu/st_asm330lhhx/Makefile | 12 + .../iio/stm/imu/st_asm330lhhx/st_asm330lhhx.h | 1013 +++++ .../imu/st_asm330lhhx/st_asm330lhhx_buffer.c | 680 ++++ .../imu/st_asm330lhhx/st_asm330lhhx_core.c | 2441 ++++++++++++ .../imu/st_asm330lhhx/st_asm330lhhx_events.c | 633 +++ .../st_asm330lhhx/st_asm330lhhx_hwtimestamp.c | 117 + .../stm/imu/st_asm330lhhx/st_asm330lhhx_i2c.c | 75 + .../stm/imu/st_asm330lhhx/st_asm330lhhx_mlc.c | 1001 +++++ .../st_asm330lhhx/st_asm330lhhx_preload_mlc.h | 111 + .../imu/st_asm330lhhx/st_asm330lhhx_shub.c | 1136 ++++++ .../stm/imu/st_asm330lhhx/st_asm330lhhx_spi.c | 74 + drivers/iio/stm/imu/st_imu68/Kconfig | 24 + drivers/iio/stm/imu/st_imu68/Makefile | 6 + drivers/iio/stm/imu/st_imu68/st_imu68.h | 87 + .../iio/stm/imu/st_imu68/st_imu68_buffer.c | 200 + drivers/iio/stm/imu/st_imu68/st_imu68_core.c | 662 ++++ drivers/iio/stm/imu/st_imu68/st_imu68_i2c.c | 76 + drivers/iio/stm/imu/st_imu68/st_imu68_spi.c | 112 + drivers/iio/stm/imu/st_ism330dhcx/Kconfig | 22 + drivers/iio/stm/imu/st_ism330dhcx/Makefile | 7 + .../iio/stm/imu/st_ism330dhcx/st_ism330dhcx.h | 582 +++ .../imu/st_ism330dhcx/st_ism330dhcx_buffer.c | 1023 +++++ .../imu/st_ism330dhcx/st_ism330dhcx_core.c | 1908 +++++++++ .../imu/st_ism330dhcx/st_ism330dhcx_embfunc.c | 620 +++ .../stm/imu/st_ism330dhcx/st_ism330dhcx_i2c.c | 97 + .../imu/st_ism330dhcx/st_ism330dhcx_shub.c | 1006 +++++ .../stm/imu/st_ism330dhcx/st_ism330dhcx_spi.c | 107 + drivers/iio/stm/imu/st_ism330dlc/Kconfig | 88 + drivers/iio/stm/imu/st_ism330dlc/Makefile | 13 + .../iio/stm/imu/st_ism330dlc/st_ism330dlc.h | 359 ++ .../imu/st_ism330dlc/st_ism330dlc_buffer.c | 552 +++ .../stm/imu/st_ism330dlc/st_ism330dlc_core.c | 2810 +++++++++++++ .../stm/imu/st_ism330dlc/st_ism330dlc_i2c.c | 182 + .../st_ism330dlc/st_ism330dlc_i2c_master.c | 1775 +++++++++ .../stm/imu/st_ism330dlc/st_ism330dlc_spi.c | 194 + .../imu/st_ism330dlc/st_ism330dlc_trigger.c | 180 + drivers/iio/stm/imu/st_lsm6ds3/Kconfig | 95 + drivers/iio/stm/imu/st_lsm6ds3/Makefile | 13 + drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3.h | 376 ++ .../stm/imu/st_lsm6ds3/st_lsm6ds3_buffer.c | 652 +++ .../iio/stm/imu/st_lsm6ds3/st_lsm6ds3_core.c | 3209 +++++++++++++++ .../iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c.c | 182 + .../imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c | 1782 +++++++++ .../iio/stm/imu/st_lsm6ds3/st_lsm6ds3_spi.c | 200 + .../stm/imu/st_lsm6ds3/st_lsm6ds3_trigger.c | 210 + drivers/iio/stm/imu/st_lsm6ds3h/Kconfig | 93 + drivers/iio/stm/imu/st_lsm6ds3h/Makefile | 13 + drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h.h | 370 ++ .../stm/imu/st_lsm6ds3h/st_lsm6ds3h_buffer.c | 644 +++ .../stm/imu/st_lsm6ds3h/st_lsm6ds3h_core.c | 3178 +++++++++++++++ .../iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c.c | 176 + .../imu/st_lsm6ds3h/st_lsm6ds3h_i2c_master.c | 1709 ++++++++ .../iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_spi.c | 194 + .../stm/imu/st_lsm6ds3h/st_lsm6ds3h_trigger.c | 207 + drivers/iio/stm/imu/st_lsm6dsm/Kconfig | 96 + drivers/iio/stm/imu/st_lsm6dsm/Makefile | 13 + drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm.h | 371 ++ .../stm/imu/st_lsm6dsm/st_lsm6dsm_buffer.c | 685 ++++ .../iio/stm/imu/st_lsm6dsm/st_lsm6dsm_core.c | 3497 +++++++++++++++++ .../iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c.c | 180 + .../imu/st_lsm6dsm/st_lsm6dsm_i2c_master.c | 1776 +++++++++ .../iio/stm/imu/st_lsm6dsm/st_lsm6dsm_spi.c | 198 + .../stm/imu/st_lsm6dsm/st_lsm6dsm_trigger.c | 244 ++ drivers/iio/stm/imu/st_lsm6dso16is/Kconfig | 26 + drivers/iio/stm/imu/st_lsm6dso16is/Makefile | 8 + .../stm/imu/st_lsm6dso16is/st_lsm6dso16is.h | 343 ++ .../imu/st_lsm6dso16is/st_lsm6dso16is_core.c | 1210 ++++++ .../imu/st_lsm6dso16is/st_lsm6dso16is_i2c.c | 65 + .../imu/st_lsm6dso16is/st_lsm6dso16is_shub.c | 961 +++++ .../imu/st_lsm6dso16is/st_lsm6dso16is_spi.c | 63 + .../st_lsm6dso16is/st_lsm6dso16is_triggers.c | 132 + drivers/iio/stm/imu/st_lsm6dsox/Kconfig | 38 + drivers/iio/stm/imu/st_lsm6dsox/Makefile | 14 + drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox.h | 864 ++++ .../stm/imu/st_lsm6dsox/st_lsm6dsox_buffer.c | 717 ++++ .../stm/imu/st_lsm6dsox/st_lsm6dsox_core.c | 2307 +++++++++++ .../stm/imu/st_lsm6dsox/st_lsm6dsox_embfunc.c | 219 ++ .../imu/st_lsm6dsox/st_lsm6dsox_hwtimestamp.c | 116 + .../iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i2c.c | 95 + .../iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i3c.c | 59 + .../iio/stm/imu/st_lsm6dsox/st_lsm6dsox_mlc.c | 906 +++++ .../stm/imu/st_lsm6dsox/st_lsm6dsox_shub.c | 1102 ++++++ .../iio/stm/imu/st_lsm6dsox/st_lsm6dsox_spi.c | 94 + drivers/iio/stm/imu/st_lsm6dsrx/Kconfig | 48 + drivers/iio/stm/imu/st_lsm6dsrx/Makefile | 15 + drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx.h | 1038 +++++ .../stm/imu/st_lsm6dsrx/st_lsm6dsrx_buffer.c | 684 ++++ .../stm/imu/st_lsm6dsrx/st_lsm6dsrx_core.c | 1986 ++++++++++ .../stm/imu/st_lsm6dsrx/st_lsm6dsrx_embfunc.c | 568 +++ .../stm/imu/st_lsm6dsrx/st_lsm6dsrx_events.c | 1048 +++++ .../imu/st_lsm6dsrx/st_lsm6dsrx_hwtimestamp.c | 116 + .../iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i2c.c | 79 + .../iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i3c.c | 58 + .../iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_mlc.c | 861 ++++ .../imu/st_lsm6dsrx/st_lsm6dsrx_preload_mlc.h | 46 + .../stm/imu/st_lsm6dsrx/st_lsm6dsrx_shub.c | 1136 ++++++ .../iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_spi.c | 78 + drivers/iio/stm/imu/st_lsm6dsvx/Kconfig | 51 + drivers/iio/stm/imu/st_lsm6dsvx/Makefile | 13 + drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx.h | 1019 +++++ .../stm/imu/st_lsm6dsvx/st_lsm6dsvx_buffer.c | 786 ++++ .../stm/imu/st_lsm6dsvx/st_lsm6dsvx_core.c | 1795 +++++++++ .../stm/imu/st_lsm6dsvx/st_lsm6dsvx_embfunc.c | 570 +++ .../stm/imu/st_lsm6dsvx/st_lsm6dsvx_events.c | 1032 +++++ .../imu/st_lsm6dsvx/st_lsm6dsvx_hwtimestamp.c | 116 + .../iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i2c.c | 79 + .../iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i3c.c | 58 + .../iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_mlc.c | 827 ++++ .../imu/st_lsm6dsvx/st_lsm6dsvx_preload_mlc.h | 20 + .../stm/imu/st_lsm6dsvx/st_lsm6dsvx_qvar.c | 371 ++ .../stm/imu/st_lsm6dsvx/st_lsm6dsvx_shub.c | 976 +++++ .../iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_spi.c | 78 + drivers/iio/stm/magnetometer/Kconfig | 57 + drivers/iio/stm/magnetometer/Makefile | 15 + drivers/iio/stm/magnetometer/st_mag3d.h | 72 + .../iio/stm/magnetometer/st_mag3d_buffer.c | 169 + drivers/iio/stm/magnetometer/st_mag3d_core.c | 465 +++ drivers/iio/stm/magnetometer/st_mag3d_i2c.c | 90 + drivers/iio/stm/magnetometer/st_mag3d_spi.c | 124 + .../iio/stm/magnetometer/st_mag40_buffer.c | 180 + drivers/iio/stm/magnetometer/st_mag40_core.c | 389 ++ drivers/iio/stm/magnetometer/st_mag40_core.h | 135 + drivers/iio/stm/magnetometer/st_mag40_i2c.c | 180 + drivers/iio/stm/magnetometer/st_mag40_spi.c | 185 + drivers/iio/stm/pressure/Kconfig | 88 + drivers/iio/stm/pressure/Makefile | 29 + drivers/iio/stm/pressure/st_lps22df.h | 138 + drivers/iio/stm/pressure/st_lps22df_buffer.c | 307 ++ drivers/iio/stm/pressure/st_lps22df_core.c | 461 +++ drivers/iio/stm/pressure/st_lps22df_i2c.c | 105 + drivers/iio/stm/pressure/st_lps22df_spi.c | 102 + drivers/iio/stm/pressure/st_lps22hb.h | 88 + drivers/iio/stm/pressure/st_lps22hb_buffer.c | 288 ++ drivers/iio/stm/pressure/st_lps22hb_core.c | 496 +++ drivers/iio/stm/pressure/st_lps22hb_i2c.c | 90 + drivers/iio/stm/pressure/st_lps22hb_spi.c | 97 + drivers/iio/stm/pressure/st_lps22hh.h | 110 + drivers/iio/stm/pressure/st_lps22hh_buffer.c | 309 ++ drivers/iio/stm/pressure/st_lps22hh_core.c | 492 +++ drivers/iio/stm/pressure/st_lps22hh_i2c.c | 94 + drivers/iio/stm/pressure/st_lps22hh_spi.c | 101 + drivers/iio/stm/pressure/st_lps33hw.h | 88 + drivers/iio/stm/pressure/st_lps33hw_buffer.c | 286 ++ drivers/iio/stm/pressure/st_lps33hw_core.c | 496 +++ drivers/iio/stm/pressure/st_lps33hw_i2c.c | 90 + drivers/iio/stm/pressure/st_lps33hw_spi.c | 97 + drivers/iio/stm/temperature/Kconfig | 17 + drivers/iio/stm/temperature/Makefile | 6 + drivers/iio/stm/temperature/stts22h.c | 624 +++ drivers/iio/stm/temperature/stts22h.h | 104 + drivers/iio/stm/tmos/Kconfig | 11 + drivers/iio/stm/tmos/Makefile | 7 + drivers/iio/stm/tmos/st_sths34pf80/Kconfig | 25 + drivers/iio/stm/tmos/st_sths34pf80/Makefile | 5 + .../stm/tmos/st_sths34pf80/st_sths34pf80.h | 403 ++ .../tmos/st_sths34pf80/st_sths34pf80_core.c | 2157 ++++++++++ .../tmos/st_sths34pf80/st_sths34pf80_i2c.c | 82 + .../tmos/st_sths34pf80/st_sths34pf80_spi.c | 81 + include/linux/platform_data/stm/ism303dac.h | 17 + include/linux/platform_data/stm/lis2ds12.h | 22 + include/linux/platform_data/stm/lis2hh12.h | 17 + stm_iio_configs/acc33_defconfig | 3 + stm_iio_configs/asm330lhhx_defconfig | 6 + stm_iio_configs/common_defconfig | 1 + stm_iio_configs/imu68_defconfig | 3 + stm_iio_configs/ism303dac_defconfig | 4 + stm_iio_configs/ism330dhcx_defconfig | 4 + stm_iio_configs/ism330dlc_defconfig | 8 + stm_iio_configs/lis2duxs12_defconfig | 5 + stm_iio_configs/lis2dw12_defconfig | 3 + stm_iio_configs/lis2hh12_defconfig | 3 + stm_iio_configs/lis3dhh_defconfig | 1 + stm_iio_configs/lisds12_defconfig | 4 + stm_iio_configs/lps22df_defconfig | 3 + stm_iio_configs/lps22hb_defconfig | 3 + stm_iio_configs/lps22hh_defconfig | 3 + stm_iio_configs/lps33hw_defconfig | 3 + stm_iio_configs/lsm6ds3_defconfig | 9 + stm_iio_configs/lsm6ds3h_defconfig | 9 + stm_iio_configs/lsm6dsm_defconfig | 9 + stm_iio_configs/lsm6dso16is_defconfig | 3 + stm_iio_configs/lsm6dsox_defconfig | 5 + stm_iio_configs/lsm6dsrx_defconfig | 6 + stm_iio_configs/lsm6dsvx_defconfig | 6 + stm_iio_configs/mag3d_defconfig | 3 + stm_iio_configs/mag40_defconfig | 3 + stm_iio_configs/sths34pf80_defconfig | 3 + stm_iio_configs/stts22h_defconfig | 1 + ...-stm-Added-iio-type-patch-for-STMEMS.patch | 49 + ...-STM-sensors-drivers-to-build-system.patch | 34 + ...ew-iio-event-used-for-timesync-logic.patch | 26 + ...-stm-Added-iio-type-patch-for-STMEMS.patch | 49 + ...-STM-sensors-drivers-to-build-system.patch | 33 + ...ew-iio-event-used-for-timesync-logic.patch | 26 + ...-stm-Added-iio-type-patch-for-STMEMS.patch | 49 + ...-STM-sensors-drivers-to-build-system.patch | 34 + ...ew-iio-event-used-for-timesync-logic.patch | 35 + ...-stm-Added-iio-type-patch-for-STMEMS.patch | 49 + ...-STM-sensors-drivers-to-build-system.patch | 33 + ...ew-iio-event-used-for-timesync-logic.patch | 26 + ...-stm-Added-iio-type-patch-for-STMEMS.patch | 49 + ...-STM-sensors-drivers-to-build-system.patch | 34 + ...ew-iio-event-used-for-timesync-logic.patch | 26 + ...-stm-Added-iio-type-patch-for-STMEMS.patch | 49 + ...-STM-sensors-drivers-to-build-system.patch | 33 + ...ew-iio-event-used-for-timesync-logic.patch | 26 + 283 files changed, 94019 insertions(+) create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_acc33.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_ism303dac.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_lis2ds12.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_lis2duxs12.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_lis2dw12.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_lis2hh12.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/accel/st_lis3dhh.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_asm330lhhx.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_imu68.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_ism330dhcx.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_ism330dlc.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3h.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsm.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dso16is.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsox.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsrx.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsvx.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag3d.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag40.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/pressure/st_lps22df.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hb.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hh.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/pressure/st_lps33hw.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/st_lis2hh12.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/temperature/hts221.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/temperature/stts22h.txt create mode 100644 Documentation/devicetree/bindings/iio/stm/tmos/sths34pf80.txt create mode 100644 README.md create mode 100644 drivers/iio/stm/Kconfig create mode 100644 drivers/iio/stm/Makefile create mode 100644 drivers/iio/stm/accel/Kconfig create mode 100644 drivers/iio/stm/accel/Makefile create mode 100644 drivers/iio/stm/accel/st_acc33.h create mode 100644 drivers/iio/stm/accel/st_acc33_buffer.c create mode 100644 drivers/iio/stm/accel/st_acc33_core.c create mode 100644 drivers/iio/stm/accel/st_acc33_i2c.c create mode 100644 drivers/iio/stm/accel/st_acc33_spi.c create mode 100644 drivers/iio/stm/accel/st_ism303dac_accel.h create mode 100644 drivers/iio/stm/accel/st_ism303dac_accel_buffer.c create mode 100644 drivers/iio/stm/accel/st_ism303dac_accel_core.c create mode 100644 drivers/iio/stm/accel/st_ism303dac_accel_i2c.c create mode 100644 drivers/iio/stm/accel/st_ism303dac_accel_spi.c create mode 100644 drivers/iio/stm/accel/st_ism303dac_accel_trigger.c create mode 100644 drivers/iio/stm/accel/st_lis2ds12.h create mode 100644 drivers/iio/stm/accel/st_lis2ds12_buffer.c create mode 100644 drivers/iio/stm/accel/st_lis2ds12_core.c create mode 100644 drivers/iio/stm/accel/st_lis2ds12_i2c.c create mode 100644 drivers/iio/stm/accel/st_lis2ds12_spi.c create mode 100644 drivers/iio/stm/accel/st_lis2ds12_trigger.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12.h create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_basicfunc.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_buffer.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_core.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_embfunc.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_i2c.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_i3c.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_mlc.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_qvar.c create mode 100644 drivers/iio/stm/accel/st_lis2duxs12_spi.c create mode 100644 drivers/iio/stm/accel/st_lis2dw12.h create mode 100644 drivers/iio/stm/accel/st_lis2dw12_buffer.c create mode 100644 drivers/iio/stm/accel/st_lis2dw12_core.c create mode 100644 drivers/iio/stm/accel/st_lis2dw12_i2c.c create mode 100644 drivers/iio/stm/accel/st_lis2dw12_spi.c create mode 100644 drivers/iio/stm/accel/st_lis2hh12.h create mode 100644 drivers/iio/stm/accel/st_lis2hh12_buffer.c create mode 100644 drivers/iio/stm/accel/st_lis2hh12_core.c create mode 100644 drivers/iio/stm/accel/st_lis2hh12_i2c.c create mode 100644 drivers/iio/stm/accel/st_lis2hh12_spi.c create mode 100644 drivers/iio/stm/accel/st_lis2hh12_trigger.c create mode 100644 drivers/iio/stm/accel/st_lis3dhh.c create mode 100644 drivers/iio/stm/accel/st_lis3dhh.h create mode 100644 drivers/iio/stm/accel/st_lis3dhh_buffer.c create mode 100644 drivers/iio/stm/imu/Kconfig create mode 100644 drivers/iio/stm/imu/Makefile create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/Kconfig create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/Makefile create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx.h create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_buffer.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_core.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_events.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_hwtimestamp.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_i2c.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_mlc.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_preload_mlc.h create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_shub.c create mode 100644 drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_spi.c create mode 100644 drivers/iio/stm/imu/st_imu68/Kconfig create mode 100644 drivers/iio/stm/imu/st_imu68/Makefile create mode 100644 drivers/iio/stm/imu/st_imu68/st_imu68.h create mode 100644 drivers/iio/stm/imu/st_imu68/st_imu68_buffer.c create mode 100644 drivers/iio/stm/imu/st_imu68/st_imu68_core.c create mode 100644 drivers/iio/stm/imu/st_imu68/st_imu68_i2c.c create mode 100644 drivers/iio/stm/imu/st_imu68/st_imu68_spi.c create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/Kconfig create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/Makefile create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx.h create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_buffer.c create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_core.c create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_embfunc.c create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_i2c.c create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_shub.c create mode 100644 drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_spi.c create mode 100644 drivers/iio/stm/imu/st_ism330dlc/Kconfig create mode 100644 drivers/iio/stm/imu/st_ism330dlc/Makefile create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc.h create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_buffer.c create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_core.c create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c.c create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c_master.c create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_spi.c create mode 100644 drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_trigger.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3.h create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_spi.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_trigger.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h.h create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c_master.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_spi.c create mode 100644 drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_trigger.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c_master.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_spi.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_trigger.c create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is.h create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_shub.c create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_spi.c create mode 100644 drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_triggers.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_embfunc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_hwtimestamp.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i3c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_mlc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_shub.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_spi.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_embfunc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_events.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_hwtimestamp.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i3c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_mlc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_preload_mlc.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_shub.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_spi.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/Kconfig create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/Makefile create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_buffer.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_core.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_embfunc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_events.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_hwtimestamp.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i2c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i3c.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_mlc.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_preload_mlc.h create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_qvar.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_shub.c create mode 100644 drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_spi.c create mode 100644 drivers/iio/stm/magnetometer/Kconfig create mode 100644 drivers/iio/stm/magnetometer/Makefile create mode 100644 drivers/iio/stm/magnetometer/st_mag3d.h create mode 100644 drivers/iio/stm/magnetometer/st_mag3d_buffer.c create mode 100644 drivers/iio/stm/magnetometer/st_mag3d_core.c create mode 100644 drivers/iio/stm/magnetometer/st_mag3d_i2c.c create mode 100644 drivers/iio/stm/magnetometer/st_mag3d_spi.c create mode 100644 drivers/iio/stm/magnetometer/st_mag40_buffer.c create mode 100644 drivers/iio/stm/magnetometer/st_mag40_core.c create mode 100644 drivers/iio/stm/magnetometer/st_mag40_core.h create mode 100644 drivers/iio/stm/magnetometer/st_mag40_i2c.c create mode 100644 drivers/iio/stm/magnetometer/st_mag40_spi.c create mode 100644 drivers/iio/stm/pressure/Kconfig create mode 100644 drivers/iio/stm/pressure/Makefile create mode 100644 drivers/iio/stm/pressure/st_lps22df.h create mode 100644 drivers/iio/stm/pressure/st_lps22df_buffer.c create mode 100644 drivers/iio/stm/pressure/st_lps22df_core.c create mode 100644 drivers/iio/stm/pressure/st_lps22df_i2c.c create mode 100644 drivers/iio/stm/pressure/st_lps22df_spi.c create mode 100644 drivers/iio/stm/pressure/st_lps22hb.h create mode 100644 drivers/iio/stm/pressure/st_lps22hb_buffer.c create mode 100644 drivers/iio/stm/pressure/st_lps22hb_core.c create mode 100644 drivers/iio/stm/pressure/st_lps22hb_i2c.c create mode 100644 drivers/iio/stm/pressure/st_lps22hb_spi.c create mode 100644 drivers/iio/stm/pressure/st_lps22hh.h create mode 100644 drivers/iio/stm/pressure/st_lps22hh_buffer.c create mode 100644 drivers/iio/stm/pressure/st_lps22hh_core.c create mode 100644 drivers/iio/stm/pressure/st_lps22hh_i2c.c create mode 100644 drivers/iio/stm/pressure/st_lps22hh_spi.c create mode 100644 drivers/iio/stm/pressure/st_lps33hw.h create mode 100644 drivers/iio/stm/pressure/st_lps33hw_buffer.c create mode 100644 drivers/iio/stm/pressure/st_lps33hw_core.c create mode 100644 drivers/iio/stm/pressure/st_lps33hw_i2c.c create mode 100644 drivers/iio/stm/pressure/st_lps33hw_spi.c create mode 100644 drivers/iio/stm/temperature/Kconfig create mode 100644 drivers/iio/stm/temperature/Makefile create mode 100644 drivers/iio/stm/temperature/stts22h.c create mode 100644 drivers/iio/stm/temperature/stts22h.h create mode 100644 drivers/iio/stm/tmos/Kconfig create mode 100644 drivers/iio/stm/tmos/Makefile create mode 100644 drivers/iio/stm/tmos/st_sths34pf80/Kconfig create mode 100644 drivers/iio/stm/tmos/st_sths34pf80/Makefile create mode 100644 drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80.h create mode 100644 drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_core.c create mode 100644 drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_i2c.c create mode 100644 drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_spi.c create mode 100644 include/linux/platform_data/stm/ism303dac.h create mode 100644 include/linux/platform_data/stm/lis2ds12.h create mode 100644 include/linux/platform_data/stm/lis2hh12.h create mode 100644 stm_iio_configs/acc33_defconfig create mode 100644 stm_iio_configs/asm330lhhx_defconfig create mode 100644 stm_iio_configs/common_defconfig create mode 100644 stm_iio_configs/imu68_defconfig create mode 100644 stm_iio_configs/ism303dac_defconfig create mode 100644 stm_iio_configs/ism330dhcx_defconfig create mode 100644 stm_iio_configs/ism330dlc_defconfig create mode 100644 stm_iio_configs/lis2duxs12_defconfig create mode 100644 stm_iio_configs/lis2dw12_defconfig create mode 100644 stm_iio_configs/lis2hh12_defconfig create mode 100644 stm_iio_configs/lis3dhh_defconfig create mode 100644 stm_iio_configs/lisds12_defconfig create mode 100644 stm_iio_configs/lps22df_defconfig create mode 100644 stm_iio_configs/lps22hb_defconfig create mode 100644 stm_iio_configs/lps22hh_defconfig create mode 100644 stm_iio_configs/lps33hw_defconfig create mode 100644 stm_iio_configs/lsm6ds3_defconfig create mode 100644 stm_iio_configs/lsm6ds3h_defconfig create mode 100644 stm_iio_configs/lsm6dsm_defconfig create mode 100644 stm_iio_configs/lsm6dso16is_defconfig create mode 100644 stm_iio_configs/lsm6dsox_defconfig create mode 100644 stm_iio_configs/lsm6dsrx_defconfig create mode 100644 stm_iio_configs/lsm6dsvx_defconfig create mode 100644 stm_iio_configs/mag3d_defconfig create mode 100644 stm_iio_configs/mag40_defconfig create mode 100644 stm_iio_configs/sths34pf80_defconfig create mode 100644 stm_iio_configs/stts22h_defconfig create mode 100644 stm_iio_patches/4.14.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch create mode 100644 stm_iio_patches/4.14.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch create mode 100644 stm_iio_patches/4.14.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch create mode 100644 stm_iio_patches/4.19.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch create mode 100644 stm_iio_patches/4.19.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch create mode 100644 stm_iio_patches/4.19.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch create mode 100644 stm_iio_patches/4.9.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch create mode 100644 stm_iio_patches/4.9.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch create mode 100644 stm_iio_patches/4.9.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch create mode 100644 stm_iio_patches/5.10.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch create mode 100644 stm_iio_patches/5.10.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch create mode 100644 stm_iio_patches/5.10.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch create mode 100644 stm_iio_patches/5.15.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch create mode 100644 stm_iio_patches/5.15.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch create mode 100644 stm_iio_patches/5.15.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch create mode 100644 stm_iio_patches/5.4.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch create mode 100644 stm_iio_patches/5.4.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch create mode 100644 stm_iio_patches/5.4.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_acc33.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_acc33.txt new file mode 100644 index 000000000000..c11cfff633d2 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_acc33.txt @@ -0,0 +1,46 @@ +* st_acc33 driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis2dh_accel" + "st,lis2dh12_accel" + "st,lis3dh_accel" + "st,lsm303agr_accel" + "st,iis2dh_accel" + +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 the spi bindings: +- spi-3wire: use sensor spi interface with 3 wires. + +Optional properties for all bus drivers: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +- st,module_id: module identifier. + If a sensor is supported by two different drivers (for example + accel by this driver and magn by mag40 driver), module_id + should be used by both drivers and should have the same id. + 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). + +Example for an spi device node: + +lis2dh-accel@0 { + compatible = "st,lis2dh_accel"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_ism303dac.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_ism303dac.txt new file mode 100644 index 000000000000..fd611225640b --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_ism303dac.txt @@ -0,0 +1,37 @@ +* ism303dac driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,ism303dac_accel" + +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 the spi bindings: +- spi-3wire: use sensor spi interface with 3 wires. + +Optional properties for all bus drivers: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +Example for an spi device node: + +ism303dac-accel@0 { + compatible = "st,ism303dac_accel"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_lis2ds12.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2ds12.txt new file mode 100644 index 000000000000..0c223c2821fd --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2ds12.txt @@ -0,0 +1,39 @@ +* lis2ds12 driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis2ds12" + "st,lsm303ah" + "st,lis2dg" + +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 the spi bindings: +- spi-3wire: use sensor spi interface with 3 wires. + +Optional properties for all bus drivers: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +Example for an spi device node: + +lis2ds12-accel@0 { + compatible = "st,lis2ds12"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_lis2duxs12.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2duxs12.txt new file mode 100644 index 000000000000..e54a9a28e4f5 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2duxs12.txt @@ -0,0 +1,49 @@ +* lis2duxs12 driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis2dux12" + "st,lis2duxs12" + +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: +- st,int-pin: the pin on the package that will be used to signal when + sensor data are available (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +- pd_dis_int1: disable pull down on int1 pin. +- pd_dis_int2: disable pull down on int2 pin. +- drive-open-drain: set interrupt pin in open drain (disable push-pull) + +Example for an spi device node: + +lis2duxs12-accel@0 { + compatible = "st,lis2duxs12"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,int-pin = <1>; +}; + +Example for an i2c device node (I2C slave address 0x19): + +lis2duxs12-accel@19 { + compatible = "st,lis2duxs12"; + reg = <0x19>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_lis2dw12.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2dw12.txt new file mode 100644 index 000000000000..562562372c27 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2dw12.txt @@ -0,0 +1,36 @@ +* lis2dw12 driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis2dw12" + "st,iis2dlpc" + "st,ais2ih" + +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: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +Example for an spi device node: + +lis2dw12-accel@0 { + compatible = "st,lis2dw12"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_lis2hh12.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2hh12.txt new file mode 100644 index 000000000000..5f4bbb3948f5 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_lis2hh12.txt @@ -0,0 +1,34 @@ +* lis2hh12 driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis2hh12" + +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: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +Example for an spi device node: + +lis2hh12-accel@0 { + compatible = "st,lis2hh12"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/accel/st_lis3dhh.txt b/Documentation/devicetree/bindings/iio/stm/accel/st_lis3dhh.txt new file mode 100644 index 000000000000..b76f55e396be --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/accel/st_lis3dhh.txt @@ -0,0 +1,30 @@ +* lis3dhh driver for accel MEMS sensors + +Required properties for the spi bindings: +- compatible: must be one of: + "st,lis3dhh" + "st,iis3dhhc" +- reg: the chipselect index +- spi-max-frequency: maximal bus speed, should be set to 1000000 unless + constrained by external circuitry + +Optional properties: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +Example for an spi device node: + +lis3dhh-accel@0 { + compatible = "st,lis3dhh"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_asm330lhhx.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_asm330lhhx.txt new file mode 100644 index 000000000000..57c4199847f4 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_asm330lhhx.txt @@ -0,0 +1,79 @@ +* st_asm330lhhx driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,asm330lhh" + "st,asm330lhhx" + +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). + +- st,mlc-int-pin: the pin on the package that will be used to signal + MLC/FSM event (valid values: 1 for int pin 1, 2 for + int pin 2 or 3 for both int pin, default: st,int-pin). + +- mount-matrix: mount rotation matrix. + + Refer to iio/mount-matrix.txt for details. + +- interrupts: interrupt mapping for IRQ. It should be configured + with flags IRQ_TYPE_LEVEL_HIGH or IRQ_TYPE_LEVEL_LOW. + + Refer to interrupt-controller/interrupts.txt for + generic interrupt client node bindings. + +- 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). + +- wakeup-source: https://www.kernel.org/doc/Documentation/devicetree/bindings/power/wakeup-source.txt + +Example for an spi asm330lhh device node: + +asm330lhh-imu@0 { + compatible = "st,asm330lhh"; + 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>; + st,mlc-int-pin = <2>; + st,module_id = <1>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; +}; + +Example for an i2c asm330lhhx device node (SA0 pulled down): + +asm330lhhx-imu@0x6a { + compatible = "st,asm330lhhx"; + reg = <0x6a>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + vddio-supply = <&sensors_vddio>; + vdd-supply = <&sensors_vdd>; + st,int-pin = <1>; + st,mlc-int-pin = <2>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_imu68.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_imu68.txt new file mode 100644 index 000000000000..035bc84a9799 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_imu68.txt @@ -0,0 +1,37 @@ +* st_imu68 driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6ds0" + "st,lsm9ds1" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +- 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). + +Example for an spi device node: + +lsm6ds0-imu@0 { + compatible = "st,lsm6ds0"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_ism330dhcx.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_ism330dhcx.txt new file mode 100644 index 000000000000..8f4a5f6addb8 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_ism330dhcx.txt @@ -0,0 +1,46 @@ +* st_ism330dhcx driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,ism330dhcx" + +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: +- st,int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- 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. + +- 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). + +- wakeup-source: https://www.kernel.org/doc/Documentation/devicetree/bindings/power/wakeup-source.txt + +Example for an spi device node: + +ism330dhcx-imu@0 { + compatible = "st,ism330dhcx"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <5>; + st,int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_ism330dlc.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_ism330dlc.txt new file mode 100644 index 000000000000..47d0c272e338 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_ism330dlc.txt @@ -0,0 +1,40 @@ +* st_ism330dlc driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,ism330dlc" + +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: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +- 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). + +Example for an spi device node: + +ism330dlc-imu@0 { + compatible = "st,ism330dlc"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <5>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3.txt new file mode 100644 index 000000000000..519614e0cb17 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3.txt @@ -0,0 +1,37 @@ +* st_lsm6ds3 driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6ds3" + "st,lsm6ds33" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +- 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). + +Example for an spi device node: + +lsm6ds3-imu@0 { + compatible = "st,lsm6ds3"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <5>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3h.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3h.txt new file mode 100644 index 000000000000..32c4e59953d0 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6ds3h.txt @@ -0,0 +1,36 @@ +* st_lsm6ds3h driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6ds3h" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +- 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). + +Example for an spi device node: + +lsm6ds3h-imu@0 { + compatible = "st,lsm6ds3h"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <5>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsm.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsm.txt new file mode 100644 index 000000000000..a36f354c3fed --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsm.txt @@ -0,0 +1,41 @@ +* st_lsm6dsm driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6dsm" + "st,lsm6dsl" + +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: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +- 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). + +Example for an spi device node: + +lsm6dsm-imu@0 { + compatible = "st,lsm6dsm"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; + st,module_id = <5>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dso16is.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dso16is.txt new file mode 100644 index 000000000000..0ac57f1e44a6 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dso16is.txt @@ -0,0 +1,55 @@ +* st_lsm6dso16is driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6dso16is" + +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. + +- mount-matrix: mount rotation matrix. + Refer to iio/mount-matrix.txt for details. + +- enable-sensor-hub: enable i2c master interface. Default is disabled. + +- drive-pullup-shub: enable pullup on master i2c line. + +Example for an spi device node on CS0: + +lsm6dso16is-imu@0 { + compatible = "st,lsm6dso16is"; + reg = <0x0>; + spi-max-frequency = <1000000>; + vddio-supply = <&sensors_vddio>; + vdd-supply = <&sensors_vdd>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; + drive-pullup-shub; +}; + +Example for an i2c device node when SA0 tied to ground: + +lsm6dso16is@6a { + compatible = "st,lsm6dso16is"; + reg = <0x6a>; + vddio-supply = <&sensors_vddio>; + vdd-supply = <&sensors_vdd>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; + drive-pullup-shub; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsox.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsox.txt new file mode 100644 index 000000000000..1a6b1158345f --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsox.txt @@ -0,0 +1,64 @@ +* st_lsm6dsox driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6dso" + "st,lsm6dsox" + "st,lsm6dso32" + "st,lsm6dso32x" + +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. + +- 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). + +- wakeup-source: https://www.kernel.org/doc/Documentation/devicetree/bindings/power/wakeup-source.txt + +Example for an spi device node: + +lsm6dsox-imu@0 { + compatible = "st,lsm6dsox"; + 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 = <5>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsrx.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsrx.txt new file mode 100644 index 000000000000..f54e97d0f370 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsrx.txt @@ -0,0 +1,62 @@ +* st_lsm6dsrx driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6dsr" + "st,lsm6dsrx" + +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. + +- 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). + +- wakeup-source: https://www.kernel.org/doc/Documentation/devicetree/bindings/power/wakeup-source.txt + +Example for an spi device node: + +lsm6dsox-imu@0 { + compatible = "st,lsm6dsox"; + 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>; + st,module_id = <1>; + mount-matrix = "1", "0", "0", + "0", "1", "0", + "0", "0", "1"; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsvx.txt b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsvx.txt new file mode 100644 index 000000000000..1db80a0047ca --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/imu/st_lsm6dsvx.txt @@ -0,0 +1,85 @@ +* st_lsm6dsvx driver for imu MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm6dsv16x" + "st,lsm6dsv" + +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. + +- enable-sensor-hub: enable i2c master interface. Default is disabled. + +- drive-pullup-shub: enable pull up on the i2c master interface. Default is disabled. + +- 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). + +- wakeup-source: https://www.kernel.org/doc/Documentation/devicetree/bindings/power/wakeup-source.txt + +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/Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag3d.txt b/Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag3d.txt new file mode 100644 index 000000000000..28d8fa66f584 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag3d.txt @@ -0,0 +1,41 @@ +* st_mag3d driver for magnetometer MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis3mdl_magn" + "st,lsm9ds1_magn" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +Example for an spi device node: + +lis3mdl-magn@0 { + compatible = "st,lis3mdl_magn"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; + +Example for an i2c device node: + +lis3mdl-magn@0x1e { + compatible = "st,lis3mdl_magn"; + reg = <0x1e>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; + diff --git a/Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag40.txt b/Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag40.txt new file mode 100644 index 000000000000..16a88a8ded97 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/magnetometer/st_mag40.txt @@ -0,0 +1,43 @@ +* st_mag40 driver for magnetometer MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lsm303ah_magn" + "st,lsm303agr_magn" + "st,lis2mdl_magn" + "st,ism303dac_magn" + "st,iis2mdc_magn" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +- st,module_id: module identifier. + If a sensor is supported by two different drivers (for example + accel by this driver and magn by mag40 driver), module_id + should be used by both drivers and should have the same id. + 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). + +Example for an spi device node: + +lsm303ah-magn@0 { + compatible = "st,lsm303ah_magn"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,module_id = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22df.txt b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22df.txt new file mode 100644 index 000000000000..6fc5c37c8cb8 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22df.txt @@ -0,0 +1,44 @@ +* st_lps22df driver for pressure MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lps22df" + "st,lps28dfw" + +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: +- 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. +- int-active-low: set device int pin active low (default active high) +- int-open-drain: set device int pin open in drain configuration (default + is push pull) + +Example for an spi device node: + +lps22df-pressure@0 { + compatible = "st,lps22df"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; + +Example for an i2c device node: + +lps28dfw-pressure@5c { + compatible = "st,lps28dfw"; + reg = <0x5c>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hb.txt b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hb.txt new file mode 100644 index 000000000000..adf784bca4b2 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hb.txt @@ -0,0 +1,31 @@ +* st_lps22hb driver for pressure MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lps22hb" + "st,lps22hd" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +Example for an spi device node: + +lps22hb-pressure@0 { + compatible = "st,lps22hb"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hh.txt b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hh.txt new file mode 100644 index 000000000000..591d7e23c603 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps22hh.txt @@ -0,0 +1,33 @@ +* st_lps22hh driver for pressure MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lps22ch" + "st,lps22hh" + "st,lps27hhw" + +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: +- 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. + +Example for an spi device node: + +lps22hh-pressure@0 { + compatible = "st,lps22hh"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/pressure/st_lps33hw.txt b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps33hw.txt new file mode 100644 index 000000000000..8ecc6f03ad85 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/pressure/st_lps33hw.txt @@ -0,0 +1,31 @@ +* st_lps33hw driver for pressure MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lps33hw" + "st,lps35hw" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +Example for an spi device node: + +lps33hw-pressure@0 { + compatible = "st,lps33hw"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/st_lis2hh12.txt b/Documentation/devicetree/bindings/iio/stm/st_lis2hh12.txt new file mode 100644 index 000000000000..5f4bbb3948f5 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/st_lis2hh12.txt @@ -0,0 +1,34 @@ +* lis2hh12 driver for accel MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,lis2hh12" + +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: +- st,drdy-int-pin: the pin on the package that will be used to signal + "data ready" (valid values: 1 or 2, default: 1). + +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic + interrupt client node bindings. + +Example for an spi device node: + +lis2hh12-accel@0 { + compatible = "st,lis2hh12"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; + st,drdy-int-pin = <1>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/temperature/hts221.txt b/Documentation/devicetree/bindings/iio/stm/temperature/hts221.txt new file mode 100644 index 000000000000..75671754a686 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/temperature/hts221.txt @@ -0,0 +1,30 @@ +* hts221 driver for temperature MEMS sensors + +Required properties for all bus drivers: +- compatible: must be one of: + "st,hts221" + +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: +- interrupts: interrupt mapping for IRQ. It should be configured with + flags IRQ_TYPE_LEVEL_HIGH. + + Refer to interrupt-controller/interrupts.txt for generic interrupt + client node bindings. + +Example for an spi device node: + +hts221-temperature@0 { + compatible = "st,hts221"; + reg = <0x0>; + spi-max-frequency = <1000000>; + interrupt-parent = <&gpio0>; + interrupts = <0 IRQ_TYPE_LEVEL_HIGH>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/temperature/stts22h.txt b/Documentation/devicetree/bindings/iio/stm/temperature/stts22h.txt new file mode 100644 index 000000000000..31d661932829 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/temperature/stts22h.txt @@ -0,0 +1,12 @@ +* stts22h driver for temperature MEMS sensors + +Required properties for the i2c bindings: +- compatible: must be one of: + "st,stts22h" + +Example for an i2c device node: + +stts22h-temperature@38 { + compatible = "st,stts22h"; + reg = <0x38>; +}; diff --git a/Documentation/devicetree/bindings/iio/stm/tmos/sths34pf80.txt b/Documentation/devicetree/bindings/iio/stm/tmos/sths34pf80.txt new file mode 100644 index 000000000000..1acd967e95bc --- /dev/null +++ b/Documentation/devicetree/bindings/iio/stm/tmos/sths34pf80.txt @@ -0,0 +1,47 @@ +* sths34pf80 driver for TMOS MEMS sensors + +Required properties for bindings: +- compatible: must be "st,sths34pf80" + +Required properties for the i2c bindings: +- reg: i2c slave address. + +Required properties for the spi bindings: +- reg: the SPI chip select index. +- spi-max-frequency: maximal bus speed, should be set to 10000000 unless + constrained by external circuitry. +- spi-3wire: device supports 3-wire SPI communication only. + +Optional properties for all bus drivers: +- interrupt-parent: identifies the controller node as interrupt-parent + (please see kernel Documentation). +- interrupts: interrupt pin (please see kernel Documentation). +- 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. +- interrupts: interrupt mapping for IRQ. It should be configured + with flags IRQ_TYPE_EDGE_BOTH. + + Refer to interrupt-controller/interrupts.txt for + generic interrupt client node bindings. + +Example for an i2c device node (i2c address 0x5a): + +sths34pf80@5a { + compatible = "st,sths34pf80"; + reg = <0x5a>; + interrupt-parent = <&gpio>; + interrupts = <26 IRQ_TYPE_EDGE_BOTH>; +}; + +Example for an spi device node: + +sths34pf80@0 { + spi-max-frequency = <1000000>; + compatible = "st,sths34pf80"; + reg = <0>; + spi-3wire; + interrupt-parent = <&gpio>; + interrupts = <13 IRQ_TYPE_EDGE_BOTH>; +}; diff --git a/README.md b/README.md new file mode 100644 index 000000000000..28b95dc6d412 --- /dev/null +++ b/README.md @@ -0,0 +1,84 @@ +# Introduction +This repository contains STMicroelectronics MEMS IIO drivers for Linux/Android kernels. + +## Source code integration +From your kernel source code directory add the git remote (i.e. stmems_iio_github) for this repository: +```bash +git remote add stmems_iio_github \ + https://github.com/STMicroelectronics/st-mems-android-linux-drivers-iio.git +``` + +Fetch the just added remote: +```bash +git fetch stmems_iio_github +``` + +There are now two ways to integrate the drivers code into the kernel target branch: +* merge (**suggested strategy**) +* rebase + +### merge +Merge the stmems_iio_github/master with your target kernel source branch (i.e branch linux-5.4.y): + +```bash +git merge --allow-unrelated-histories \ + linux-5.4.y \ + stmems_iio_github/master +``` + +### rebase +Rebase the stmems_iio_github/master on top of your target kernel source branch (i.e branch linux-5.4.y): + +```bash +git rebase --no-fork-point \ + linux-5.4.y \ + stmems_iio_github/master +``` + +Note: older git versions (i.e.: 2.7.4) would require to use sligthly different options: + +```bash +git merge --no-fork-point \ + linux-5.4.y \ + stmems_iio_github/master +``` + +```bash +git rebase -Xno-renames \ + --no-fork-point \ + linux-5.4.y \ + stmems_iio_github/master +``` + +## Apply patches +Now that drivers code has been added to the target kernel branch, few patches needs to be added in order to: +* add STM drivers into Kconfig & Makefile systems +* patch IIO framework with custom events with custom events, channels and devices + +Apply the patches available in the just added repository (i.e branch linux-5.4.y): + +```bash +git am stm_iio_patches/5.4.y/*-stm-*.patch +``` + +## Configuration +A folder named stm_iio_configs should now be available with the default configs for the supported drivers. + +### Modify target defconfig +Sensors defconfig can be appended to the board defconfig (i.e. if your current configuration file is arch/arm/configs/stm32_defconfig): + +```bash +cat stm_iio_configs/lsm6dsm_defconfig >> arch/arm/configs/stm32_defconfig +``` + +Alternatively, it can be done at build time without altering the board config file, as follow. + +### Merge configuration +Driver config can be merged into current target pre-configured kernel using a script available in the kernel itself: + +```bash +export ARCH=arm +export CROSS_COMPILE=arm-linux-gnu- +scripts/kconfig/merge_config.sh -n .config stm_iio_configs/lsm6dsm_defconfig +``` + diff --git a/drivers/iio/stm/Kconfig b/drivers/iio/stm/Kconfig new file mode 100644 index 000000000000..3863345f9241 --- /dev/null +++ b/drivers/iio/stm/Kconfig @@ -0,0 +1,20 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# STM Industrial I/O drivers configuration +# +# When adding new entries keep the list in alphabetical order + +menuconfig IIO_STM + tristate "STM MEMS Device Drivers" + +if IIO_STM + +source "drivers/iio/stm/accel/Kconfig" +source "drivers/iio/stm/imu/Kconfig" +source "drivers/iio/stm/magnetometer/Kconfig" +source "drivers/iio/stm/pressure/Kconfig" +source "drivers/iio/stm/temperature/Kconfig" +source "drivers/iio/stm/tmos/Kconfig" + +endif # IIO_STM + diff --git a/drivers/iio/stm/Makefile b/drivers/iio/stm/Makefile new file mode 100644 index 000000000000..a552a9b645f1 --- /dev/null +++ b/drivers/iio/stm/Makefile @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# STM Makefile for Industrial I/O drivers +# +# When adding new entries keep the list in alphabetical order + +obj-$(CONFIG_IIO_STM) += accel/ +obj-$(CONFIG_IIO_STM) += imu/ +obj-$(CONFIG_IIO_STM) += magnetometer/ +obj-$(CONFIG_IIO_STM) += pressure/ +obj-$(CONFIG_IIO_STM) += temperature/ +obj-$(CONFIG_IIO_STM) += tmos/ diff --git a/drivers/iio/stm/accel/Kconfig b/drivers/iio/stm/accel/Kconfig new file mode 100644 index 000000000000..0919e0e1e4e9 --- /dev/null +++ b/drivers/iio/stm/accel/Kconfig @@ -0,0 +1,192 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Accelerometer drivers +# +# When adding new entries keep the list in alphabetical order + +menu "Accelerometers" + +config IIO_ST_ACC33 + tristate "STMicroelectronics LIS3DH/LIS2DH/LIS2DH12/LSM303AGR/IIS2DH Accelerometer driver" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_ST_ACC33_I2C if (I2C) + select IIO_ST_ACC33_SPI if (SPI_MASTER) + select IIO_BUFFER + select IIO_KFIFO_BUF + help + Say yes here to build support for STMicroelectronics accelerometers: + LIS3DH, LIS2DH, LIS2DH12, LSM303AGR, IIS2DH. + + This driver can also be built as a module. If so, will be created + these modules: + - st_ac33_core (core functions for the driver [it is mandatory]); + - st_ac33_i2c (necessary for the I2C devices [optional*]); + - st_ac33_spi (necessary for the SPI devices [optional*]); + + (*) one of these is necessary to do something. + +config IIO_ST_ACC33_I2C + tristate + depends on IIO_ST_ACC33 + +config IIO_ST_ACC33_SPI + tristate + depends on IIO_ST_ACC33 + +menuconfig IIO_ST_LIS2DS12 + tristate "STMicroelectronics LIS2DS12/LSM303AH Accelerometer Driver" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_LIS2DS12_I2C if (I2C) + select IIO_ST_LIS2DS12_SPI if (SPI) + help + Say yes here to build support for the LIS2DS12 and LSM303AH + accelerometers. + +if IIO_ST_LIS2DS12 +config IIO_ST_LIS2DS12_I2C + tristate + depends on IIO_ST_LIS2DS12 + depends on I2C + +config IIO_ST_LIS2DS12_SPI + tristate + depends on IIO_ST_LIS2DS12 + depends on SPI + +config ST_LIS2DS12_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on IIO_ST_LIS2DS12 + range 0 1536 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +endif #IIO_ST_LIS2DS12 + +config IIO_ST_LIS2DW12 + tristate "STMicroelectronics LIS2DW12/IIS2DLPC Accelerometer Driver" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_ST_LIS2DW12_I2C if (I2C) + select IIO_ST_LIS2DW12_SPI if (SPI) + help + Say yes here to build support for the LIS2DW12 and IIS2DLPC accelerometer. + + This driver can also be built as a module. If so, will be created + these modules: + - st_lisdw12 (core functions for the driver [it is mandatory]); + - st_lisdw12_i2c (necessary for the I2C devices [optional*]); + - st_lisdw12_spi (necessary for the SPI devices [optional*]); + + (*) one of these is necessary to do something. + +config IIO_ST_LIS2DW12_I2C + tristate + depends on IIO_ST_LIS2DW12 + depends on I2C + +config IIO_ST_LIS2DW12_SPI + tristate + depends on IIO_ST_LIS2DW12 + depends on SPI + +menuconfig IIO_ST_ISM303DAC_ACCEL + tristate "STMicroelectronics ISM303DAC Accelerometer Driver" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_ISM303DAC_ACCEL_I2C if (I2C) + select IIO_ST_ISM303DAC_ACCEL_SPI if (SPI) + help + Say yes here to build support for the ISM303DAC + accelerometers. + +config IIO_ST_ISM303DAC_ACCEL_I2C + tristate + depends on IIO_ST_ISM303DAC_ACCEL + depends on I2C + +config IIO_ST_ISM303DAC_ACCEL_SPI + tristate + depends on IIO_ST_ISM303DAC_ACCEL + depends on SPI + +config ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on IIO_ST_ISM303DAC_ACCEL + range 0 1536 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +config IIO_ST_LIS3DHH + tristate "STMicroelectronics LIS3DHH/IIS3DHHC Accelerometer driver" + depends on SPI_MASTER && SYSFS + select IIO_BUFFER + select IIO_KFIFO_BUF + help + Say yes here to build support for STMicroelectronics LIS3DHH and + IIS3DHHC accelerometers + + This driver can also be built as a module. If so, will be named + st_lis3dhh + +config IIO_ST_LIS2HH12 + tristate "STMicroelectronics LIS2HH12 Accelerometer driver" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_LIS2HH12_I2C if (I2C) + select IIO_ST_LIS2HH12_SPI if (SPI) + help + Say yes here to build support for the LIS2HH12 accelerometer. + +config IIO_ST_LIS2HH12_I2C + tristate + depends on IIO_ST_LIS2HH12 + depends on I2C + +config IIO_ST_LIS2HH12_SPI + tristate + depends on IIO_ST_LIS2HH12 + +config IIO_ST_LIS2DUXS12 + tristate "STMicroelectronics LIS2DUX12/LIS2DUXS12 sensors" + depends on (I2C || SPI || I3C) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_ST_LIS2DUXS12_I2C if (I2C) + select IIO_ST_LIS2DUXS12_SPI if (SPI_MASTER) + select IIO_ST_LIS2DUXS12_I3C if (I3C) + help + Say yes here to build support for STMicroelectronics LIS2DUX(S)12 accel + sensors. + + To compile this driver as a module, choose M here: the module + will be called lis2duxs12. + +config IIO_ST_LIS2DUXS12_I2C + tristate + select REGMAP_I2C + depends on IIO_ST_LIS2DUXS12 + +config IIO_ST_LIS2DUXS12_SPI + tristate + select REGMAP_SPI + depends on IIO_ST_LIS2DUXS12 + +config IIO_ST_LIS2DUXS12_I3C + tristate + select REGMAP_I3C + depends on IIO_ST_LIS2DUXS12 + +endmenu diff --git a/drivers/iio/stm/accel/Makefile b/drivers/iio/stm/accel/Makefile new file mode 100644 index 000000000000..2bc7d8c26bf1 --- /dev/null +++ b/drivers/iio/stm/accel/Makefile @@ -0,0 +1,44 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for industrial I/O accelerometer drivers +# + +# When adding new entries keep the list in alphabetical order +obj-$(CONFIG_IIO_ST_LIS2DS12) += lis2ds12.o +obj-$(CONFIG_IIO_ST_LIS2DS12_I2C) += st_lis2ds12_i2c.o +obj-$(CONFIG_IIO_ST_LIS2DS12_SPI) += st_lis2ds12_spi.o + +lis2ds12-y += st_lis2ds12_core.o st_lis2ds12_buffer.o st_lis2ds12_trigger.o + +st_acc33-y := st_acc33_core.o st_acc33_buffer.o +obj-$(CONFIG_IIO_ST_ACC33) += st_acc33.o +obj-$(CONFIG_IIO_ST_ACC33_I2C) += st_acc33_i2c.o +obj-$(CONFIG_IIO_ST_ACC33_SPI) += st_acc33_spi.o + +st_lis2dw12-y:= st_lis2dw12_core.o st_lis2dw12_buffer.o +obj-$(CONFIG_IIO_ST_LIS2DW12) += st_lis2dw12.o +obj-$(CONFIG_IIO_ST_LIS2DW12_I2C) += st_lis2dw12_i2c.o +obj-$(CONFIG_IIO_ST_LIS2DW12_SPI) += st_lis2dw12_spi.o + +ism303dac_accel-y += st_ism303dac_accel_core.o st_ism303dac_accel_buffer.o \ + st_ism303dac_accel_trigger.o +obj-$(CONFIG_IIO_ST_ISM303DAC_ACCEL) += ism303dac_accel.o +obj-$(CONFIG_IIO_ST_ISM303DAC_ACCEL_I2C) += st_ism303dac_accel_i2c.o +obj-$(CONFIG_IIO_ST_ISM303DAC_ACCEL_SPI) += st_ism303dac_accel_spi.o + +st-lis3dhh-y := st_lis3dhh.o st_lis3dhh_buffer.o +obj-$(CONFIG_IIO_ST_LIS3DHH) += st-lis3dhh.o + +lis2hh12-y += st_lis2hh12_core.o st_lis2hh12_buffer.o st_lis2hh12_trigger.o +obj-$(CONFIG_IIO_ST_LIS2HH12) += lis2hh12.o +obj-$(CONFIG_IIO_ST_LIS2HH12_I2C) += st_lis2hh12_i2c.o +obj-$(CONFIG_IIO_ST_LIS2HH12_SPI) += st_lis2hh12_spi.o + +st_lis2duxs12-y := st_lis2duxs12_core.o st_lis2duxs12_buffer.o \ + st_lis2duxs12_mlc.o st_lis2duxs12_embfunc.o \ + st_lis2duxs12_basicfunc.o st_lis2duxs12_qvar.o + +obj-$(CONFIG_IIO_ST_LIS2DUXS12) += st_lis2duxs12.o +obj-$(CONFIG_IIO_ST_LIS2DUXS12_I2C) += st_lis2duxs12_i2c.o +obj-$(CONFIG_IIO_ST_LIS2DUXS12_SPI) += st_lis2duxs12_spi.o +obj-$(CONFIG_IIO_ST_LIS2DUXS12_I3C) += st_lis2duxs12_i3c.o diff --git a/drivers/iio/stm/accel/st_acc33.h b/drivers/iio/stm/accel/st_acc33.h new file mode 100644 index 000000000000..4ef6b401db2c --- /dev/null +++ b/drivers/iio/stm/accel/st_acc33.h @@ -0,0 +1,88 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_acc33 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_ACC33_H +#define ST_ACC33_H + +#define LIS2DH_DEV_NAME "lis2dh_accel" +#define LIS2DH12_DEV_NAME "lis2dh12_accel" +#define LIS3DH_DEV_NAME "lis3dh_accel" +#define LSM303AGR_DEV_NAME "lsm303agr_accel" +#define IIS2DH_DEV_NAME "iis2dh_accel" + +#define ST_ACC33_DATA_SIZE 6 + +#include + +#define ST_ACC33_RX_MAX_LENGTH 96 +#define ST_ACC33_TX_MAX_LENGTH 8 + +struct st_acc33_transfer_buffer { + u8 rx_buf[ST_ACC33_RX_MAX_LENGTH]; + u8 tx_buf[ST_ACC33_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_acc33_transfer_function { + int (*read)(struct device *dev, u8 addr, int len, u8 *data); + int (*write)(struct device *dev, u8 addr, int len, u8 *data); +}; + +enum st_acc33_fifo_mode { + ST_ACC33_FIFO_BYPASS = 0x0, + ST_ACC33_FIFO_STREAM = 0x2, +}; + +struct st_acc33_hw { + struct device *dev; + const char *name; + int irq; + + u32 module_id; + + struct mutex fifo_lock; + struct mutex lock; + + u8 watermark; + u32 gain; + u16 odr; + + u64 samples; + u8 std_level; + + s64 delta_ts; + s64 ts_irq; + s64 ts; + + struct iio_dev *iio_dev; + + const struct st_acc33_transfer_function *tf; + struct st_acc33_transfer_buffer tb; +}; + +int st_acc33_write_with_mask(struct st_acc33_hw *hw, u8 addr, u8 mask, + u8 val); +int st_acc33_set_enable(struct st_acc33_hw *hw, bool enable); +int st_acc33_probe(struct device *device, int irq, const char *name, + const struct st_acc33_transfer_function *tf_ops); +int st_acc33_fifo_setup(struct st_acc33_hw *hw); +ssize_t st_acc33_flush_hwfifo(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_acc33_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_acc33_get_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + char *buf); +ssize_t st_acc33_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +int st_acc33_update_watermark(struct st_acc33_hw *hw, u8 watermark); + +#endif /* ST_ACC33_H */ diff --git a/drivers/iio/stm/accel/st_acc33_buffer.c b/drivers/iio/stm/accel/st_acc33_buffer.c new file mode 100644 index 000000000000..06e86e8dbc27 --- /dev/null +++ b/drivers/iio/stm/accel/st_acc33_buffer.c @@ -0,0 +1,290 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_acc33 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_acc33.h" + +#define REG_CTRL5_ACC_ADDR 0x24 +#define REG_CTRL5_ACC_FIFO_EN_MASK BIT(6) +#define REG_CTRL5_ACC_BOOT_MASK BIT(7) + +#define REG_FIFO_CTRL_REG 0x2e +#define REG_FIFO_CTRL_REG_WTM_MASK GENMASK(4, 0) +#define REG_FIFO_CTRL_MODE_MASK GENMASK(7, 6) + +#define REG_FIFO_SRC_ADDR 0x2f +#define REG_FIFO_SRC_OVR_MASK BIT(6) +#define REG_FIFO_SRC_FSS_MASK GENMASK(4, 0) + +#define ST_ACC33_MAX_WATERMARK 28 + +static inline s64 st_acc33_get_timestamp(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +#define ST_ACC33_EWMA_DIV 128 +static inline s64 st_acc33_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_ACC33_EWMA_DIV - weight) * diff, + ST_ACC33_EWMA_DIV); + + return old + incr; +} + +static int st_acc33_set_fifo_mode(struct st_acc33_hw *hw, + enum st_acc33_fifo_mode mode) +{ + return st_acc33_write_with_mask(hw, REG_FIFO_CTRL_REG, + REG_FIFO_CTRL_MODE_MASK, mode); +} + +static int st_acc33_read_fifo(struct st_acc33_hw *hw) +{ + u8 iio_buff[ALIGN(ST_ACC33_DATA_SIZE, sizeof(s64)) + sizeof(s64)]; + u8 buff[ST_ACC33_RX_MAX_LENGTH], data, nsamples; + struct iio_dev *iio_dev = hw->iio_dev; + struct iio_chan_spec const *ch = iio_dev->channels; + int i, err, word_len, fifo_len, read_len = 0; + s64 delta_ts; + + err = hw->tf->read(hw->dev, REG_FIFO_SRC_ADDR, sizeof(data), &data); + if (err < 0) + return err; + + delta_ts = div_s64(hw->delta_ts, (hw->watermark + 1)); + nsamples = data & REG_FIFO_SRC_FSS_MASK; + fifo_len = nsamples * ST_ACC33_DATA_SIZE; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buff)); + err = hw->tf->read(hw->dev, ch[0].address, word_len, buff); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_ACC33_DATA_SIZE) { + if (unlikely(++hw->samples <= hw->std_level)) { + hw->ts += delta_ts; + continue; + } + + memcpy(iio_buff, &buff[i], ST_ACC33_DATA_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buff, + hw->ts); + hw->ts += delta_ts; + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_acc33_flush_hwfifo(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_acc33_hw *hw = iio_priv(iio_dev); + s64 code; + int err; + + mutex_lock(&hw->fifo_lock); + + err = st_acc33_read_fifo(hw); + hw->ts_irq = st_acc33_get_timestamp(iio_dev); + + mutex_unlock(&hw->fifo_lock); + + code = IIO_UNMOD_EVENT_CODE(IIO_ACCEL, -1, + IIO_EV_TYPE_FIFO_FLUSH, + IIO_EV_DIR_EITHER); + iio_push_event(iio_dev, code, hw->ts_irq); + + return err < 0 ? err : size; +} + +ssize_t st_acc33_get_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_acc33_hw *hw = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", hw->watermark); +} + +int st_acc33_update_watermark(struct st_acc33_hw *hw, u8 watermark) +{ + return st_acc33_write_with_mask(hw, REG_FIFO_CTRL_REG, + REG_FIFO_CTRL_REG_WTM_MASK, + watermark); +} + +ssize_t st_acc33_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_acc33_hw *hw = iio_priv(iio_dev); + int err, val; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock; + } + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto unlock; + + if (val < 1 || val > ST_ACC33_MAX_WATERMARK) { + err = -EINVAL; + goto unlock; + } + + err = st_acc33_update_watermark(hw, val); + if (err < 0) + goto unlock; + + hw->watermark = val; + +unlock: + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : size; +} + +ssize_t st_acc33_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_ACC33_MAX_WATERMARK); +} + +static irqreturn_t st_acc33_buffer_handler_irq(int irq, void *private) +{ + struct st_acc33_hw *hw = private; + struct iio_dev *iio_dev = hw->iio_dev; + u8 ewma_level; + s64 ts; + + ewma_level = hw->odr >= 100 ? 120 : 96; + ts = st_acc33_get_timestamp(iio_dev); + hw->delta_ts = st_acc33_ewma(hw->delta_ts, ts - hw->ts_irq, + ewma_level); + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_acc33_buffer_handler_thread(int irq, void *private) +{ + struct st_acc33_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_acc33_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_acc33_update_fifo(struct st_acc33_hw *hw, bool enable) +{ + struct iio_dev *iio_dev = hw->iio_dev; + enum st_acc33_fifo_mode mode; + + int err; + + if (enable) { + hw->ts_irq = hw->ts = st_acc33_get_timestamp(iio_dev); + hw->delta_ts = div_s64(1000000000LL, hw->odr) * + (hw->watermark + 1); + hw->samples = 0; + } + + err = st_acc33_write_with_mask(hw, REG_CTRL5_ACC_ADDR, + REG_CTRL5_ACC_FIFO_EN_MASK, enable); + if (err < 0) + return err; + + mode = enable ? ST_ACC33_FIFO_STREAM : ST_ACC33_FIFO_BYPASS; + err = st_acc33_set_fifo_mode(hw, mode); + if (err < 0) + return err; + + return st_acc33_set_enable(hw, enable); +} + +static int st_acc33_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_acc33_update_fifo(iio_priv(iio_dev), true); +} + +static int st_acc33_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_acc33_update_fifo(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_acc33_buffer_ops = { + .preenable = st_acc33_buffer_preenable, + .postdisable = st_acc33_buffer_postdisable, +}; + +int st_acc33_fifo_setup(struct st_acc33_hw *hw) +{ + struct iio_dev *iio_dev = hw->iio_dev; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + int ret; + + ret = devm_request_threaded_irq(hw->dev, hw->irq, + st_acc33_buffer_handler_irq, + st_acc33_buffer_handler_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + hw->name, hw); + if (ret) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return ret; + } + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + ret = devm_iio_kfifo_buffer_setup(hw->dev, iio_dev, + INDIO_BUFFER_SOFTWARE, + &st_acc33_buffer_ops); + if (ret) + return ret; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(iio_dev, buffer); + iio_dev->setup_ops = &st_acc33_buffer_ops; + iio_dev->modes |= INDIO_BUFFER_SOFTWARE; +#endif /* LINUX_VERSION_CODE */ + + return 0; +} diff --git a/drivers/iio/stm/accel/st_acc33_core.c b/drivers/iio/stm/accel/st_acc33_core.c new file mode 100644 index 000000000000..c0cffc538594 --- /dev/null +++ b/drivers/iio/stm/accel/st_acc33_core.c @@ -0,0 +1,533 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_acc33 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_acc33.h" + +#define REG_WHOAMI_ADDR 0x0f +#define REG_WHOAMI_VAL 0x33 + +#define REG_CTRL1_ADDR 0x20 +#define REG_CTRL1_ODR_MASK GENMASK(7, 4) + +#define REG_CTRL3_ADDR 0x22 +#define REG_CTRL3_I1_OVR_MASK BIT(1) +#define REG_CTRL3_I1_WTM_MASK BIT(2) +#define REG_CTRL3_I1_DRDY1_MASK BIT(4) + +#define REG_CTRL4_ADDR 0x23 +#define REG_CTRL4_BDU_MASK BIT(7) +#define REG_CTRL4_FS_MASK GENMASK(5, 4) + +#define REG_CTRL6_ACC_ADDR 0x25 + +#define REG_OUTX_L_ADDR 0x28 +#define REG_OUTY_L_ADDR 0x2a +#define REG_OUTZ_L_ADDR 0x2c + +#define ST_ACC33_FS_2G IIO_G_TO_M_S_2(980) +#define ST_ACC33_FS_4G IIO_G_TO_M_S_2(1950) +#define ST_ACC33_FS_8G IIO_G_TO_M_S_2(3900) +#define ST_ACC33_FS_16G IIO_G_TO_M_S_2(11720) + +#define ST_ACC33_DATA_CHANNEL(addr, modx, scan_idx) \ +{ \ + .type = IIO_ACCEL, \ + .address = addr, \ + .modified = 1, \ + .channel2 = modx, \ + .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 = 's', \ + .realbits = 12, \ + .storagebits = 16, \ + .shift = 4, \ + .endianness = IIO_LE, \ + }, \ +} + +#define ST_ACC33_FLUSH_CHANNEL() \ +{ \ + .type = IIO_ACCEL, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_acc33_fifo_flush_event, \ + .num_event_specs = 1, \ +} + +struct st_acc33_std_entry { + u16 odr; + u8 val; +}; + +struct st_acc33_std_entry st_acc33_std_table[] = { + { 1, 3 }, + { 10, 10 }, + { 25, 18 }, + { 50, 24 }, + { 100, 24 }, + { 200, 32 }, + { 400, 48 }, +}; + +struct st_acc33_odr { + u32 hz; + u8 val; +}; + +static const struct st_acc33_odr st_acc33_odr_table[] = { + { 1, 0x01 }, /* 1Hz */ + { 10, 0x02 }, /* 10Hz */ + { 25, 0x03 }, /* 25Hz */ + { 50, 0x04 }, /* 50Hz */ + { 100, 0x05 }, /* 100Hz */ + { 200, 0x06 }, /* 200Hz */ + { 400, 0x07 }, /* 400Hz */ +}; + +struct st_acc33_fs { + u32 gain; + u8 val; +}; + +static const struct st_acc33_fs st_acc33_fs_table[] = { + { ST_ACC33_FS_2G, 0x0 }, + { ST_ACC33_FS_4G, 0x1 }, + { ST_ACC33_FS_8G, 0x2 }, + { ST_ACC33_FS_16G, 0x3 }, +}; + +const struct iio_event_spec st_acc33_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_acc33_channels[] = { + ST_ACC33_DATA_CHANNEL(REG_OUTX_L_ADDR, IIO_MOD_X, 0), + ST_ACC33_DATA_CHANNEL(REG_OUTY_L_ADDR, IIO_MOD_Y, 1), + ST_ACC33_DATA_CHANNEL(REG_OUTZ_L_ADDR, IIO_MOD_Z, 2), + ST_ACC33_FLUSH_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +int st_acc33_write_with_mask(struct st_acc33_hw *hw, u8 addr, u8 mask, + u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + mutex_unlock(&hw->lock); + + return err; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = hw->tf->write(hw->dev, addr, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to write %02x register\n", addr); + mutex_unlock(&hw->lock); + + return err; + } + + mutex_unlock(&hw->lock); + + return 0; +} + +static int st_acc33_get_odr_val(u16 odr, u8 *val) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_acc33_odr_table); i++) + if (st_acc33_odr_table[i].hz == odr) + break; + + if (i == ARRAY_SIZE(st_acc33_odr_table)) + return -EINVAL; + + *val = st_acc33_odr_table[i].val; + + return 0; +} + +static int st_acc33_set_std_level(struct st_acc33_hw *hw, u16 odr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_acc33_std_table); i++) + if (st_acc33_std_table[i].odr == odr) + break; + + if (i == ARRAY_SIZE(st_acc33_std_table)) + return -EINVAL; + + hw->std_level = st_acc33_std_table[i].val; + + return 0; +} + +static int st_acc33_update_odr(struct st_acc33_hw *hw, u16 odr) +{ + int err; + u8 val; + + err = st_acc33_get_odr_val(odr, &val); + if (err < 0) + return err; + + return st_acc33_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_ODR_MASK, val); +} + +int st_acc33_set_enable(struct st_acc33_hw *hw, bool enable) +{ + int err; + + if (enable) + err = st_acc33_update_odr(hw, hw->odr); + else + err = st_acc33_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_ODR_MASK, 0); + + return err < 0 ? err : 0; +} + +static int st_acc33_set_fs(struct st_acc33_hw *hw, u32 gain) +{ + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_acc33_fs_table); i++) { + if (st_acc33_fs_table[i].gain == gain) + break; + } + + if (i == ARRAY_SIZE(st_acc33_fs_table)) + return -EINVAL; + + err = st_acc33_write_with_mask(hw, REG_CTRL4_ADDR, + REG_CTRL4_FS_MASK, + st_acc33_fs_table[i].val); + if (err < 0) + return err; + + hw->gain = gain; + + return 0; +} + +static int st_acc33_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_acc33_hw *hw = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + u8 data[2]; + int err, delay; + + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + + err = st_acc33_set_enable(hw, true); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + /* sample to discard, 3 * odr us */ + delay = 3000000 / hw->odr; + usleep_range(delay, delay + 1); + + err = hw->tf->read(hw->dev, ch->address, 2, data); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + err = st_acc33_set_enable(hw, false); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(data); + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&iio_dev->mlock); + + ret = IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = hw->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = hw->odr; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_acc33_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_acc33_hw *hw = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_acc33_set_fs(hw, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_acc33_set_std_level(hw, val); + if (err < 0) + break; + + err = st_acc33_get_odr_val(val, &data); + if (!err) + hw->odr = val; + break; + } + default: + err = -EINVAL; + break; + } + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_acc33_get_sampling_frequency_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_acc33_odr_table); i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_acc33_odr_table[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_acc33_get_scale_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_acc33_fs_table); i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + st_acc33_fs_table[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_acc33_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_acc33_hw *hw = iio_priv(iio_dev); + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_acc33_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_acc33_get_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_acc33_get_hwfifo_watermark, + st_acc33_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_acc33_get_max_hwfifo_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_acc33_flush_hwfifo, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_acc33_get_module_id, NULL, 0); + +static struct attribute *st_acc33_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.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.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_acc33_attribute_group = { + .attrs = st_acc33_attributes, +}; + +static const struct iio_info st_acc33_info = { + .attrs = &st_acc33_attribute_group, + .read_raw = st_acc33_read_raw, + .write_raw = st_acc33_write_raw, +}; + +static int st_acc33_check_whoami(struct st_acc33_hw *hw) +{ + u8 data; + int err; + + err = hw->tf->read(hw->dev, REG_WHOAMI_ADDR, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != REG_WHOAMI_VAL) { + dev_err(hw->dev, "wrong whoami {%02x-%02x}\n", + data, REG_WHOAMI_VAL); + return -ENODEV; + } + + return 0; +} + +static int st_acc33_init_device(struct st_acc33_hw *hw) +{ + int err; + + err = st_acc33_set_fs(hw, ST_ACC33_FS_4G); + if (err < 0) + return err; + + err = st_acc33_write_with_mask(hw, REG_CTRL4_ADDR, + REG_CTRL4_BDU_MASK, 1); + if (err < 0) + return err; + + err = st_acc33_update_watermark(hw, hw->watermark); + if (err < 0) + return err; + + return st_acc33_write_with_mask(hw, REG_CTRL3_ADDR, + REG_CTRL3_I1_WTM_MASK, 1); +} + +static void st_acc33_get_properties(struct st_acc33_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", &hw->module_id)) { + hw->module_id = 1; + } +} + +static int st_acc33_init_interface(struct st_acc33_hw *hw) +{ + struct device_node *np = hw->dev->of_node; + + if (np && of_find_property(np, "spi-3wire", NULL)) { + u8 data = 0x1; + + return hw->tf->write(hw->dev, REG_CTRL4_ADDR, + sizeof(data), &data); + } else { + return 0; + } +} + +int st_acc33_probe(struct device *device, int irq, const char *name, + const struct st_acc33_transfer_function *tf_ops) +{ + struct st_acc33_hw *hw; + struct iio_dev *iio_dev; + int err; + + iio_dev = devm_iio_device_alloc(device, sizeof(*hw)); + if (!iio_dev) + return -ENOMEM; + + dev_set_drvdata(device, (void *)iio_dev); + + iio_dev->channels = st_acc33_channels; + iio_dev->num_channels = ARRAY_SIZE(st_acc33_channels); + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->info = &st_acc33_info; + iio_dev->dev.parent = device; + iio_dev->name = name; + + hw = iio_priv(iio_dev); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->lock); + + hw->odr = st_acc33_odr_table[0].hz; + hw->watermark = 1; + hw->dev = device; + hw->tf = tf_ops; + hw->name = name; + hw->irq = irq; + hw->iio_dev = iio_dev; + + err = st_acc33_init_interface(hw); + if (err < 0) + return err; + + err = st_acc33_check_whoami(hw); + if (err < 0) + return err; + + st_acc33_get_properties(hw); + + err = st_acc33_init_device(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_acc33_fifo_setup(hw); + if (err < 0) + return err; + } + + return devm_iio_device_register(hw->dev, iio_dev); +} +EXPORT_SYMBOL(st_acc33_probe); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_acc33 sensor driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_acc33_i2c.c b/drivers/iio/stm/accel/st_acc33_i2c.c new file mode 100644 index 000000000000..b256dd929565 --- /dev/null +++ b/drivers/iio/stm/accel/st_acc33_i2c.c @@ -0,0 +1,122 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_acc33 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_acc33.h" + +#define I2C_AUTO_INCREMENT BIT(7) + +static int st_acc33_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + if (len > 1) + addr |= I2C_AUTO_INCREMENT; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_acc33_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + if (len > 1) + addr |= I2C_AUTO_INCREMENT; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_acc33_transfer_function st_acc33_transfer_fn = { + .read = st_acc33_i2c_read, + .write = st_acc33_i2c_write, +}; + +static int st_acc33_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_acc33_probe(&client->dev, client->irq, client->name, + &st_acc33_transfer_fn); +} + +static const struct of_device_id st_acc33_i2c_of_match[] = { + { + .compatible = "st,lis2dh_accel", + .data = LIS2DH_DEV_NAME, + }, + { + .compatible = "st,lis2dh12_accel", + .data = LIS2DH12_DEV_NAME, + }, + { + .compatible = "st,lis3dh_accel", + .data = LIS3DH_DEV_NAME, + }, + { + .compatible = "st,lsm303agr_accel", + .data = LSM303AGR_DEV_NAME, + }, + { + .compatible = "st,iis2dh_accel", + .data = IIS2DH_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_acc33_i2c_of_match); + +static const struct i2c_device_id st_acc33_i2c_id_table[] = { + { LIS2DH_DEV_NAME }, + { LIS2DH12_DEV_NAME }, + { LIS3DH_DEV_NAME }, + { LSM303AGR_DEV_NAME }, + { IIS2DH_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_acc33_i2c_id_table); + +static struct i2c_driver st_acc33_driver = { + .driver = { + .name = "st_acc33_i2c", + .of_match_table = of_match_ptr(st_acc33_i2c_of_match), + }, + .probe = st_acc33_i2c_probe, + .id_table = st_acc33_i2c_id_table, +}; +module_i2c_driver(st_acc33_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_acc33 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_acc33_spi.c b/drivers/iio/stm/accel/st_acc33_spi.c new file mode 100644 index 000000000000..dfdf7fbce88e --- /dev/null +++ b/drivers/iio/stm/accel/st_acc33_spi.c @@ -0,0 +1,129 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_acc33 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_acc33.h" + +#define SENSORS_SPI_READ BIT(7) +#define SPI_AUTO_INCREMENT BIT(6) + +static int st_acc33_spi_read(struct device *device, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(device); + struct iio_dev *iio_dev = spi_get_drvdata(spi); + struct st_acc33_hw *hw = iio_priv(iio_dev); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (len > 1) + addr |= SPI_AUTO_INCREMENT; + + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_acc33_spi_write(struct device *device, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(device); + struct iio_dev *iio_dev = spi_get_drvdata(spi); + struct st_acc33_hw *hw = iio_priv(iio_dev); + + if (len >= ST_ACC33_TX_MAX_LENGTH) + return -ENOMEM; + + if (len > 1) + addr |= SPI_AUTO_INCREMENT; + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_acc33_transfer_function st_acc33_transfer_fn = { + .read = st_acc33_spi_read, + .write = st_acc33_spi_write, +}; + +static int st_acc33_spi_probe(struct spi_device *spi) +{ + return st_acc33_probe(&spi->dev, spi->irq, spi->modalias, + &st_acc33_transfer_fn); +} + +static const struct of_device_id st_acc33_spi_of_match[] = { + { + .compatible = "st,lis2dh_accel", + .data = LIS2DH_DEV_NAME, + }, + { + .compatible = "st,lis2dh12_accel", + .data = LIS2DH12_DEV_NAME, + }, + { + .compatible = "st,lis3dh_accel", + .data = LIS3DH_DEV_NAME, + }, + { + .compatible = "st,lsm303agr_accel", + .data = LSM303AGR_DEV_NAME, + }, + { + .compatible = "st,iis2dh_accel", + .data = IIS2DH_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_acc33_spi_of_match); + +static const struct spi_device_id st_acc33_spi_id_table[] = { + { LIS2DH_DEV_NAME }, + { LIS2DH12_DEV_NAME }, + { LIS3DH_DEV_NAME }, + { LSM303AGR_DEV_NAME }, + { IIS2DH_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_acc33_spi_id_table); + +static struct spi_driver st_acc33_driver = { + .driver = { + .name = "st_acc33_spi", + .of_match_table = of_match_ptr(st_acc33_spi_of_match), + }, + .probe = st_acc33_spi_probe, + .id_table = st_acc33_spi_id_table, +}; +module_spi_driver(st_acc33_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_acc33 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_ism303dac_accel.h b/drivers/iio/stm/accel/st_ism303dac_accel.h new file mode 100644 index 000000000000..308695b8a304 --- /dev/null +++ b/drivers/iio/stm/accel/st_ism303dac_accel.h @@ -0,0 +1,301 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics ism303dac driver + * + * MEMS Software Solutions Team + * + * Copyright 2018 STMicroelectronics Inc. + */ + +#ifndef __ISM303DAC_H +#define __ISM303DAC_H + +#include +#include +#include +#include + +#define ISM303DAC_WHO_AM_I_ADDR 0x0f +#define ISM303DAC_WHO_AM_I_DEF 0x43 +#define ISM303DAC_CTRL1_ADDR 0x20 +#define ISM303DAC_CTRL2_ADDR 0x21 +#define ISM303DAC_CTRL3_ADDR 0x22 +#define ISM303DAC_CTRL4_INT1_PAD_ADDR 0x23 +#define ISM303DAC_CTRL5_INT2_PAD_ADDR 0x24 +#define ISM303DAC_FIFO_CTRL_ADDR 0x25 +#define ISM303DAC_OUTX_L_ADDR 0x28 +#define ISM303DAC_OUTY_L_ADDR 0x2a +#define ISM303DAC_OUTZ_L_ADDR 0x2c +#define ISM303DAC_TAP_THS_6D_ADDR 0x31 +#define ISM303DAC_WAKE_UP_THS_ADDR 0x33 +#define ISM303DAC_FREE_FALL_ADDR 0x35 +#define ISM303DAC_TAP_SRC_ADDR 0x38 +#define ISM303DAC_FUNC_CTRL_ADDR 0x3f +#define ISM303DAC_FIFO_THS_ADDR 0x2e +#define ISM303DAC_FIFO_THS_MASK 0xff +#define ISM303DAC_ODR_ADDR ISM303DAC_CTRL1_ADDR +#define ISM303DAC_ODR_MASK 0xf0 +#define ISM303DAC_ODR_POWER_OFF_VAL 0x00 +#define ISM303DAC_ODR_1HZ_LP_VAL 0x08 +#define ISM303DAC_ODR_12HZ_LP_VAL 0x09 +#define ISM303DAC_ODR_25HZ_LP_VAL 0x0a +#define ISM303DAC_ODR_50HZ_LP_VAL 0x0b +#define ISM303DAC_ODR_100HZ_LP_VAL 0x0c +#define ISM303DAC_ODR_200HZ_LP_VAL 0x0d +#define ISM303DAC_ODR_400HZ_LP_VAL 0x0e +#define ISM303DAC_ODR_800HZ_LP_VAL 0x0f +#define ISM303DAC_ODR_LP_LIST_NUM 9 + +#define ISM303DAC_ODR_12_5HZ_HR_VAL 0x01 +#define ISM303DAC_ODR_25HZ_HR_VAL 0x02 +#define ISM303DAC_ODR_50HZ_HR_VAL 0x03 +#define ISM303DAC_ODR_100HZ_HR_VAL 0x04 +#define ISM303DAC_ODR_200HZ_HR_VAL 0x05 +#define ISM303DAC_ODR_400HZ_HR_VAL 0x06 +#define ISM303DAC_ODR_800HZ_HR_VAL 0x07 +#define ISM303DAC_ODR_HR_LIST_NUM 8 + +#define ISM303DAC_FS_ADDR ISM303DAC_CTRL1_ADDR +#define ISM303DAC_FS_MASK 0x0c +#define ISM303DAC_FS_2G_VAL 0x00 +#define ISM303DAC_FS_4G_VAL 0x02 +#define ISM303DAC_FS_8G_VAL 0x03 +#define ISM303DAC_FS_16G_VAL 0x01 + +/* Advanced Configuration Registers */ +#define ISM303DAC_FUNC_CFG_ENTER_ADDR ISM303DAC_CTRL2_ADDR +#define ISM303DAC_FUNC_CFG_EXIT_ADDR 0x3F +#define ISM303DAC_FUNC_CFG_EN_MASK 0x10 + +#define ISM303DAC_SIM_ADDR ISM303DAC_CTRL2_ADDR +#define ISM303DAC_SIM_MASK 0x01 +#define ISM303DAC_ADD_INC_MASK 0x04 + +/* Sensitivity for the 16-bit data */ +#define ISM303DAC_FS_2G_GAIN IIO_G_TO_M_S_2(61) +#define ISM303DAC_FS_4G_GAIN IIO_G_TO_M_S_2(122) +#define ISM303DAC_FS_8G_GAIN IIO_G_TO_M_S_2(244) +#define ISM303DAC_FS_16G_GAIN IIO_G_TO_M_S_2(488) + +#define ISM303DAC_MODE_DEFAULT ISM303DAC_HR_MODE +#define ISM303DAC_INT1_S_TAP_MASK 0x40 +#define ISM303DAC_INT1_WAKEUP_MASK 0x20 +#define ISM303DAC_INT1_FREE_FALL_MASK 0x10 +#define ISM303DAC_INT1_TAP_MASK 0x08 +#define ISM303DAC_INT1_6D_MASK 0x04 +#define ISM303DAC_INT1_FTH_MASK 0x02 +#define ISM303DAC_INT1_DRDY_MASK 0x01 +#define ISM303DAC_INT1_EVENTS_MASK (ISM303DAC_INT1_S_TAP_MASK | \ + ISM303DAC_INT1_WAKEUP_MASK | \ + ISM303DAC_INT1_FREE_FALL_MASK | \ + ISM303DAC_INT1_TAP_MASK | \ + ISM303DAC_INT1_6D_MASK | \ + ISM303DAC_INT1_FTH_MASK | \ + ISM303DAC_INT1_DRDY_MASK) +#define ISM303DAC_INT2_ON_INT1_MASK 0x20 +#define ISM303DAC_INT2_FTH_MASK 0x02 +#define ISM303DAC_INT2_DRDY_MASK 0x01 +#define ISM303DAC_INT2_EVENTS_MASK (ISM303DAC_INT2_FTH_MASK | \ + ISM303DAC_INT2_DRDY_MASK) +#define ISM303DAC_WAKE_UP_THS_WU_MASK 0x3f +#define ISM303DAC_WAKE_UP_THS_WU_DEFAULT 0x02 +#define ISM303DAC_FREE_FALL_THS_MASK 0x07 +#define ISM303DAC_FREE_FALL_DUR_MASK 0xF8 +#define ISM303DAC_FREE_FALL_THS_DEFAULT 0x01 +#define ISM303DAC_FREE_FALL_DUR_DEFAULT 0x01 +#define ISM303DAC_BDU_ADDR ISM303DAC_CTRL1_ADDR +#define ISM303DAC_BDU_MASK 0x01 +#define ISM303DAC_SOFT_RESET_ADDR ISM303DAC_CTRL2_ADDR +#define ISM303DAC_SOFT_RESET_MASK 0x40 +#define ISM303DAC_LIR_ADDR ISM303DAC_CTRL3_ADDR +#define ISM303DAC_LIR_MASK 0x04 +#define ISM303DAC_TAP_AXIS_ADDR ISM303DAC_CTRL3_ADDR +#define ISM303DAC_TAP_AXIS_MASK 0x38 +#define ISM303DAC_TAP_AXIS_ANABLE_ALL 0x07 +#define ISM303DAC_TAP_THS_ADDR ISM303DAC_TAP_THS_6D_ADDR +#define ISM303DAC_TAP_THS_MASK 0x1f +#define ISM303DAC_TAP_THS_DEFAULT 0x09 +#define ISM303DAC_INT2_ON_INT1_ADDR ISM303DAC_CTRL5_INT2_PAD_ADDR +#define ISM303DAC_INT2_ON_INT1_MASK 0x20 +#define ISM303DAC_FIFO_MODE_ADDR ISM303DAC_FIFO_CTRL_ADDR +#define ISM303DAC_FIFO_MODE_MASK 0xe0 +#define ISM303DAC_FIFO_MODE_BYPASS 0x00 +#define ISM303DAC_FIFO_MODE_CONTINUOS 0x06 +#define ISM303DAC_OUT_XYZ_SIZE 8 + +#define ISM303DAC_SELFTEST_ADDR ISM303DAC_CTRL3_ADDR +#define ISM303DAC_SELFTEST_MASK 0xc0 +#define ISM303DAC_SELFTEST_NORMAL 0x00 +#define ISM303DAC_SELFTEST_POS_SIGN 0x01 +#define ISM303DAC_SELFTEST_NEG_SIGN 0x02 + +#define ISM303DAC_FIFO_SRC 0x2f +#define ISM303DAC_FIFO_SRC_DIFF_MASK 0x20 + +#define ISM303DAC_FIFO_NUM_AXIS 3 +#define ISM303DAC_FIFO_BYTE_X_AXIS 2 +#define ISM303DAC_FIFO_BYTE_FOR_SAMPLE (ISM303DAC_FIFO_NUM_AXIS * \ + ISM303DAC_FIFO_BYTE_X_AXIS) +#define ISM303DAC_TIMESTAMP_SIZE 8 + +#define ISM303DAC_STATUS_ADDR 0x27 +#define ISM303DAC_STATUS_DUP_ADDR 0x36 +#define ISM303DAC_WAKE_UP_IA_MASK 0x40 +#define ISM303DAC_DOUBLE_TAP_MASK 0x10 +#define ISM303DAC_TAP_MASK 0x08 +#define ISM303DAC_6D_IA_MASK 0x04 +#define ISM303DAC_FF_IA_MASK 0x02 +#define ISM303DAC_DRDY_MASK 0x01 +#define ISM303DAC_EVENT_MASK (ISM303DAC_WAKE_UP_IA_MASK | \ + ISM303DAC_DOUBLE_TAP_MASK | \ + ISM303DAC_TAP_MASK | \ + ISM303DAC_6D_IA_MASK | \ + ISM303DAC_FF_IA_MASK) +#define ISM303DAC_FIFO_SRC_ADDR 0x2f +#define ISM303DAC_FIFO_SRC_FTH_MASK 0x80 + +#define ISM303DAC_EN_BIT 0x01 +#define ISM303DAC_DIS_BIT 0x00 +#define ISM303DAC_ACCEL_ODR 1 +#define ISM303DAC_DEFAULT_ACCEL_FS 2 +#define ISM303DAC_FF_ODR 25 +#define ISM303DAC_TAP_ODR 400 +#define ISM303DAC_WAKEUP_ODR 25 +#define ISM303DAC_ACTIVITY_ODR 12 +#define ISM303DAC_MAX_FIFO_LENGHT 256 +#define ISM303DAC_MAX_FIFO_THS (ISM303DAC_MAX_FIFO_LENGHT - 1) +#define ISM303DAC_MAX_CHANNEL_SPEC 5 +#define ISM303DAC_EVENT_CHANNEL_SPEC_SIZE 2 +#define ISM303DAC_MIN_DURATION_MS 1638 + +#define ISM303DAC_DEV_NAME "ism303dac_accel" +#define SET_BIT(a, b) {a |= (1 << b);} +#define RESET_BIT(a, b) {a &= ~(1 << b);} +#define CHECK_BIT(a, b) (a & (1 << b)) + +enum { + ISM303DAC_ACCEL = 0, + ISM303DAC_TAP, + ISM303DAC_DOUBLE_TAP, + ISM303DAC_SENSORS_NUMB, +}; + +#define ST_ISM303DAC_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &ism303dac_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_ISM303DAC_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + ism303dac_sysfs_get_hwfifo_enabled,\ + ism303dac_sysfs_set_hwfifo_enabled, 0); + +#define ST_ISM303DAC_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + ism303dac_sysfs_get_hwfifo_watermark,\ + ism303dac_sysfs_set_hwfifo_watermark, 0); + +#define ST_ISM303DAC_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + ism303dac_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_ISM303DAC_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + ism303dac_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_ISM303DAC_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + ism303dac_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +#define ISM303DAC_TX_MAX_LENGTH 12 +#define ISM303DAC_RX_MAX_LENGTH 8193 +#define ISM303DAC_EWMA_DIV 128 + +struct ism303dac_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[ISM303DAC_RX_MAX_LENGTH]; + u8 tx_buf[ISM303DAC_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct ism303dac_data; + +struct ism303dac_transfer_function { + int (*write)(struct ism303dac_data *cdata, u8 reg_addr, int len, + u8 *data, bool b_lock); + int (*read)(struct ism303dac_data *cdata, u8 reg_addr, int len, + u8 *data, bool b_lock); +}; + +struct ism303dac_sensor_data { + struct ism303dac_data *cdata; + const char *name; + s64 timestamp; + u8 enabled; + u32 odr; + u32 gain; + u8 sindex; + u8 sample_to_discard; +}; + +struct ism303dac_data { + const char *name; + u8 drdy_int_pin; + bool spi_3wire; + u8 selftest_status; + u8 hwfifo_enabled; + u8 hwfifo_watermark; + u8 power_mode; + u8 enabled_sensor; + u32 common_odr; + int irq; + s64 timestamp; + s64 accel_deltatime; + s64 sample_timestamp; + u8 *fifo_data; + u16 fifo_size; + u64 samples; + u8 std_level; + struct mutex fifo_lock; + struct device *dev; + struct iio_dev *iio_sensors_dev[ISM303DAC_SENSORS_NUMB]; + struct iio_trigger *iio_trig[ISM303DAC_SENSORS_NUMB]; + struct mutex regs_lock; + const struct ism303dac_transfer_function *tf; + struct ism303dac_transfer_buffer tb; +}; + +static inline s64 ism303dac_get_time_ns(struct iio_dev *iio_sensors_dev) +{ + return iio_get_time_ns(iio_sensors_dev); +} + +int ism303dac_common_probe(struct ism303dac_data *cdata, int irq); +#ifdef CONFIG_PM +int ism303dac_common_suspend(struct ism303dac_data *cdata); +int ism303dac_common_resume(struct ism303dac_data *cdata); +#endif +int ism303dac_allocate_rings(struct ism303dac_data *cdata); +int ism303dac_allocate_triggers(struct ism303dac_data *cdata, + const struct iio_trigger_ops *trigger_ops); +int ism303dac_trig_set_state(struct iio_trigger *trig, bool state); +int ism303dac_read_register(struct ism303dac_data *cdata, u8 reg_addr, + int data_len, u8 *data, bool b_lock); +int ism303dac_update_drdy_irq(struct ism303dac_sensor_data *sdata, bool state); +int ism303dac_set_enable(struct ism303dac_sensor_data *sdata, bool enable); +void ism303dac_common_remove(struct ism303dac_data *cdata, int irq); +void ism303dac_read_xyz(struct ism303dac_data *cdata); +void ism303dac_read_fifo(struct ism303dac_data *cdata, bool check_fifo_len); +void ism303dac_deallocate_rings(struct ism303dac_data *cdata); +void ism303dac_deallocate_triggers(struct ism303dac_data *cdata); + +#endif /* __ISM303DAC_H */ diff --git a/drivers/iio/stm/accel/st_ism303dac_accel_buffer.c b/drivers/iio/stm/accel/st_ism303dac_accel_buffer.c new file mode 100644 index 000000000000..5ceaf96deb73 --- /dev/null +++ b/drivers/iio/stm/accel/st_ism303dac_accel_buffer.c @@ -0,0 +1,208 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism303dac driver + * + * MEMS Software Solutions Team + * + * Copyright 2018 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_ism303dac_accel.h" + +#define ISM303DAC_ACCEL_BUFFER_SIZE \ + ALIGN(ISM303DAC_FIFO_BYTE_FOR_SAMPLE + ISM303DAC_TIMESTAMP_SIZE, \ + ISM303DAC_TIMESTAMP_SIZE) + +static void ism303dac_push_accel_data(struct ism303dac_data *cdata, + u8 *acc_buf, u16 read_length) +{ + size_t offset; + uint16_t i, j, k; + u8 buffer[ISM303DAC_ACCEL_BUFFER_SIZE], out_buf_index; + struct iio_dev *indio_dev = cdata->iio_sensors_dev[ISM303DAC_ACCEL]; + u32 delta_ts = div_s64(cdata->accel_deltatime, cdata->hwfifo_watermark); + + for (i = 0; i < read_length; i += ISM303DAC_FIFO_BYTE_FOR_SAMPLE) { + /* Skip first samples. */ + if (unlikely(++cdata->samples <= cdata->std_level)) { + cdata->sample_timestamp += delta_ts; + continue; + } + + for (j = 0, out_buf_index = 0; j < ISM303DAC_FIFO_NUM_AXIS; + j++) { + k = i + ISM303DAC_FIFO_BYTE_X_AXIS * j; + if (test_bit(j, indio_dev->active_scan_mask)) { + memcpy(&buffer[out_buf_index], + &acc_buf[k], + ISM303DAC_FIFO_BYTE_X_AXIS); + out_buf_index += ISM303DAC_FIFO_BYTE_X_AXIS; + } + } + + if (indio_dev->scan_timestamp) { + offset = indio_dev->scan_bytes / sizeof(s64) - 1; + ((s64 *)buffer)[offset] = cdata->sample_timestamp; + cdata->sample_timestamp += delta_ts; + } + + iio_push_to_buffers(indio_dev, buffer); + } +} + +void ism303dac_read_xyz(struct ism303dac_data *cdata) +{ + int err; + u8 xyz_buf[ISM303DAC_FIFO_BYTE_FOR_SAMPLE]; + + err = ism303dac_read_register(cdata, ISM303DAC_OUTX_L_ADDR, + ISM303DAC_FIFO_BYTE_FOR_SAMPLE, xyz_buf, true); + if (err < 0) + return; + + cdata->sample_timestamp = cdata->timestamp; + ism303dac_push_accel_data(cdata, xyz_buf, ISM303DAC_FIFO_BYTE_FOR_SAMPLE); +} + +void ism303dac_read_fifo(struct ism303dac_data *cdata, bool check_fifo_len) +{ + int err; + u8 fifo_src[2]; + u16 read_len; +#if (CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read, extra_bytes; +#endif /* CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO */ + + err = ism303dac_read_register(cdata, ISM303DAC_FIFO_SRC, 2, + fifo_src, true); + if (err < 0) + return; + + read_len = (fifo_src[0] & ISM303DAC_FIFO_SRC_DIFF_MASK) ? (1 << 8) : 0; + read_len |= fifo_src[1]; + read_len *= ISM303DAC_FIFO_BYTE_FOR_SAMPLE; + + if (read_len == 0) + return; + +#if (CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO == 0) + err = ism303dac_read_register(cdata, ISM303DAC_OUTX_L_ADDR, read_len, + cdata->fifo_data, true); + if (err < 0) + return; +#else /* CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + extra_bytes = (data_to_read % ISM303DAC_FIFO_BYTE_FOR_SAMPLE); + if (extra_bytes != 0) { + data_to_read -= extra_bytes; + + if (data_to_read < ISM303DAC_FIFO_BYTE_FOR_SAMPLE) + data_to_read = ISM303DAC_FIFO_BYTE_FOR_SAMPLE; + } + + err = ism303dac_read_register(cdata, ISM303DAC_OUTX_L_ADDR, data_to_read, + &cdata->fifo_data[read_len - data_remaining], true); + if (err < 0) + return; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO */ + + ism303dac_push_accel_data(cdata, cdata->fifo_data, read_len); +} + +static inline irqreturn_t ism303dac_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int ism303dac_trig_set_state(struct iio_trigger *trig, bool state) +{ + int err; + struct ism303dac_sensor_data *sdata; + + sdata = iio_priv(iio_trigger_get_drvdata(trig)); + err = ism303dac_update_drdy_irq(sdata, state); + + return (err < 0) ? err : 0; +} + +static int ism303dac_buffer_preenable(struct iio_dev *indio_dev) +{ + int err; + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + err = ism303dac_set_enable(sdata, true); + if (err < 0) + return err; + + return 0; +} + +static int ism303dac_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + err = ism303dac_set_enable(sdata, false); + if (err < 0) + return err; + + return 0; +} + +static const struct iio_buffer_setup_ops ism303dac_buffer_setup_ops = { + .preenable = &ism303dac_buffer_preenable, + .postdisable = &ism303dac_buffer_postdisable, +}; + +int ism303dac_allocate_rings(struct ism303dac_data *cdata) +{ + int err, i; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) { + err = iio_triggered_buffer_setup( + cdata->iio_sensors_dev[i], + &ism303dac_handler_empty, + NULL, + &ism303dac_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup; + } + + return 0; + +buffer_cleanup: + for (i--; i >= 0; i--) + iio_triggered_buffer_cleanup(cdata->iio_sensors_dev[i]); + + return err; +} + +void ism303dac_deallocate_rings(struct ism303dac_data *cdata) +{ + int i; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) + iio_triggered_buffer_cleanup(cdata->iio_sensors_dev[i]); +} diff --git a/drivers/iio/stm/accel/st_ism303dac_accel_core.c b/drivers/iio/stm/accel/st_ism303dac_accel_core.c new file mode 100644 index 000000000000..4c69c6dabe5b --- /dev/null +++ b/drivers/iio/stm/accel/st_ism303dac_accel_core.c @@ -0,0 +1,1252 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism303dac driver + * + * MEMS Software Solutions Team + * + * Copyright 2018 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_ism303dac_accel.h" + +#define ISM303DAC_FS_LIST_NUM 4 +enum { + ISM303DAC_LP_MODE = 0, + ISM303DAC_HR_MODE, + ISM303DAC_MODE_COUNT, +}; + +#define ISM303DAC_ADD_CHANNEL(device_type, modif, index, mod, endian, sbits,\ + rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +struct ism303dac_odr_reg { + u32 hz; + u8 value; + /* Skip samples. */ + u8 std_level; +}; + +static const struct ism303dac_odr_table_t { + u8 addr; + u8 mask; + struct ism303dac_odr_reg odr_avl[ISM303DAC_MODE_COUNT][ISM303DAC_ODR_LP_LIST_NUM]; +} ism303dac_odr_table = { + .addr = ISM303DAC_ODR_ADDR, + .mask = ISM303DAC_ODR_MASK, + + /* ODR values for Low Power Mode */ + .odr_avl[ISM303DAC_LP_MODE][0] = {.hz = 0, + .value = ISM303DAC_ODR_POWER_OFF_VAL, + .std_level = 0,}, + .odr_avl[ISM303DAC_LP_MODE][1] = {.hz = 1, + .value = ISM303DAC_ODR_1HZ_LP_VAL, + .std_level = 0,}, + .odr_avl[ISM303DAC_LP_MODE][2] = {.hz = 12, + .value = ISM303DAC_ODR_12HZ_LP_VAL, + .std_level = 4,}, + .odr_avl[ISM303DAC_LP_MODE][3] = {.hz = 25, + .value = ISM303DAC_ODR_25HZ_LP_VAL, + .std_level = 8,}, + .odr_avl[ISM303DAC_LP_MODE][4] = {.hz = 50, + .value = ISM303DAC_ODR_50HZ_LP_VAL, + .std_level = 24,}, + .odr_avl[ISM303DAC_LP_MODE][5] = {.hz = 100, + .value = ISM303DAC_ODR_100HZ_LP_VAL, + .std_level = 24,}, + .odr_avl[ISM303DAC_LP_MODE][6] = {.hz = 200, + .value = ISM303DAC_ODR_200HZ_LP_VAL, + .std_level = 32,}, + .odr_avl[ISM303DAC_LP_MODE][7] = {.hz = 400, + .value = ISM303DAC_ODR_400HZ_LP_VAL, + .std_level = 48,}, + .odr_avl[ISM303DAC_LP_MODE][8] = {.hz = 800, + .value = ISM303DAC_ODR_800HZ_LP_VAL, + .std_level = 50,}, + + /* ODR values for High Resolution Mode */ + .odr_avl[ISM303DAC_HR_MODE][0] = {.hz = 0, + .value = ISM303DAC_ODR_POWER_OFF_VAL, + .std_level = 0,}, + .odr_avl[ISM303DAC_HR_MODE][1] = {.hz = 12, + .value = ISM303DAC_ODR_12_5HZ_HR_VAL, + .std_level = 4,}, + .odr_avl[ISM303DAC_HR_MODE][2] = {.hz = 25, + .value = ISM303DAC_ODR_25HZ_HR_VAL, + .std_level = 8,}, + .odr_avl[ISM303DAC_HR_MODE][3] = {.hz = 50, + .value = ISM303DAC_ODR_50HZ_HR_VAL, + .std_level = 24,}, + .odr_avl[ISM303DAC_HR_MODE][4] = {.hz = 100, + .value = ISM303DAC_ODR_100HZ_HR_VAL, + .std_level = 24,}, + .odr_avl[ISM303DAC_HR_MODE][5] = {.hz = 200, + .value = ISM303DAC_ODR_200HZ_HR_VAL, + .std_level = 32,}, + .odr_avl[ISM303DAC_HR_MODE][6] = {.hz = 400, + .value = ISM303DAC_ODR_400HZ_HR_VAL, + .std_level = 48,}, + .odr_avl[ISM303DAC_HR_MODE][7] = {.hz = 800, + .value = ISM303DAC_ODR_800HZ_HR_VAL, + .std_level = 50,}, +}; + +struct ism303dac_fs_reg { + unsigned int gain; + u8 value; + int urv; +}; + +static struct ism303dac_fs_table { + u8 addr; + u8 mask; + struct ism303dac_fs_reg fs_avl[ISM303DAC_FS_LIST_NUM]; +} ism303dac_fs_table = { + .addr = ISM303DAC_FS_ADDR, + .mask = ISM303DAC_FS_MASK, + .fs_avl[0] = { + .gain = ISM303DAC_FS_2G_GAIN, + .value = ISM303DAC_FS_2G_VAL, + .urv = 2, + }, + .fs_avl[1] = { + .gain = ISM303DAC_FS_4G_GAIN, + .value = ISM303DAC_FS_4G_VAL, + .urv = 4, + }, + .fs_avl[2] = { + .gain = ISM303DAC_FS_8G_GAIN, + .value = ISM303DAC_FS_8G_VAL, + .urv = 8, + }, + .fs_avl[3] = { + .gain = ISM303DAC_FS_16G_GAIN, + .value = ISM303DAC_FS_16G_VAL, + .urv = 16, + }, +}; + +static const struct iio_event_spec singol_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, +}; + +const struct iio_event_spec ism303dac_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct ism303dac_sensors_table { + const char *name; + const char *description; + const u32 min_odr_hz; + const u8 iio_channel_size; + const struct iio_chan_spec iio_channel[ISM303DAC_MAX_CHANNEL_SPEC]; +} ism303dac_sensors_table[ISM303DAC_SENSORS_NUMB] = { + [ISM303DAC_ACCEL] = { + .name = "accel", + .description = "ST ISM303DAC Accelerometer Sensor", + .min_odr_hz = ISM303DAC_ACCEL_ODR, + .iio_channel = { + ISM303DAC_ADD_CHANNEL(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ISM303DAC_OUTX_L_ADDR, 's'), + ISM303DAC_ADD_CHANNEL(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ISM303DAC_OUTY_L_ADDR, 's'), + ISM303DAC_ADD_CHANNEL(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ISM303DAC_OUTZ_L_ADDR, 's'), + ST_ISM303DAC_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) + }, + .iio_channel_size = ISM303DAC_MAX_CHANNEL_SPEC, + }, + [ISM303DAC_TAP] = { + .name = "tap", + .description = "ST ISM303DAC Tap Sensor", + .min_odr_hz = ISM303DAC_TAP_ODR, + .iio_channel = { + { + .type = IIO_TAP, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = ISM303DAC_EVENT_CHANNEL_SPEC_SIZE, + }, + [ISM303DAC_DOUBLE_TAP] = { + .name = "tap_tap", + .description = "ST ISM303DAC Double Tap Sensor", + .min_odr_hz = ISM303DAC_TAP_ODR, + .iio_channel = { + { + .type = IIO_TAP_TAP, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = ISM303DAC_EVENT_CHANNEL_SPEC_SIZE, + }, +}; + +static const struct { + char *mode_str; + u8 streg_val; +} ism303dac_selftest_table[] = { + { + .mode_str = "normal-mode", + .streg_val = ISM303DAC_SELFTEST_NORMAL, + }, + { + .mode_str = "positive-sign", + .streg_val = ISM303DAC_SELFTEST_POS_SIGN, + }, + { + .mode_str = "negative-sign", + .streg_val = ISM303DAC_SELFTEST_NEG_SIGN, + }, +}; + +int ism303dac_read_register(struct ism303dac_data *cdata, u8 reg_addr, + int data_len, u8 *data, bool b_lock) +{ + return cdata->tf->read(cdata, reg_addr, data_len, data, b_lock); +} + +static int ism303dac_write_register(struct ism303dac_data *cdata, u8 reg_addr, + u8 mask, u8 data, bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = ism303dac_read_register(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} + +static int ism303dac_set_fifo_mode(struct ism303dac_data *cdata, enum fifo_mode fm) +{ + u8 reg_value; + + switch (fm) { + case BYPASS: + reg_value = ISM303DAC_FIFO_MODE_BYPASS; + break; + case CONTINUOS: + reg_value = ISM303DAC_FIFO_MODE_CONTINUOS; + break; + default: + return -EINVAL; + } + + return ism303dac_write_register(cdata, ISM303DAC_FIFO_MODE_ADDR, + ISM303DAC_FIFO_MODE_MASK, reg_value, true); +} + +static int ism303dac_set_fs(struct ism303dac_sensor_data *sdata, unsigned int fs) +{ + int err, i; + + for (i = 0; i < ISM303DAC_FS_LIST_NUM; i++) { + if (ism303dac_fs_table.fs_avl[i].urv == fs) + break; + } + + if (i == ISM303DAC_FS_LIST_NUM) + return -EINVAL; + + err = ism303dac_write_register(sdata->cdata, + ism303dac_fs_table.addr, + ism303dac_fs_table.mask, + ism303dac_fs_table.fs_avl[i].value, true); + if (err < 0) + return err; + + sdata->gain = ism303dac_fs_table.fs_avl[i].gain; + + return 0; +} + +static int ism303dac_set_selftest_mode(struct ism303dac_sensor_data *sdata, + u8 index) +{ + return ism303dac_write_register(sdata->cdata, ISM303DAC_SELFTEST_ADDR, + ISM303DAC_SELFTEST_MASK, + ism303dac_selftest_table[index].streg_val, true); +} + +static u8 ism303dac_event_irq1_value(struct ism303dac_data *cdata) +{ + u8 value = 0x0; + + if (CHECK_BIT(cdata->enabled_sensor, ISM303DAC_DOUBLE_TAP)) + value |= ISM303DAC_INT1_TAP_MASK; + + if (CHECK_BIT(cdata->enabled_sensor, ISM303DAC_TAP)) + value |= ISM303DAC_INT1_S_TAP_MASK | ISM303DAC_INT1_TAP_MASK; + + return value; +} + +static int ism303dac_write_max_odr(struct ism303dac_sensor_data *sdata) +{ + int err, i; + u32 max_odr = 0; + u8 power_mode = sdata->cdata->power_mode; + struct ism303dac_sensor_data *t_sdata; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) + if (CHECK_BIT(sdata->cdata->enabled_sensor, i)) { + t_sdata = iio_priv(sdata->cdata->iio_sensors_dev[i]); + if (t_sdata->odr > max_odr) + max_odr = t_sdata->odr; + } + + for (i = 0; i < ISM303DAC_ODR_LP_LIST_NUM; i++) { + if (ism303dac_odr_table.odr_avl[power_mode][i].hz >= max_odr) + break; + } + + if (i == ISM303DAC_ODR_LP_LIST_NUM) + return -EINVAL; + + err = ism303dac_write_register(sdata->cdata, + ism303dac_odr_table.addr, + ism303dac_odr_table.mask, + ism303dac_odr_table.odr_avl[power_mode][i].value, true); + if (err < 0) + return err; + + sdata->cdata->common_odr = max_odr; + sdata->cdata->std_level = ism303dac_odr_table.odr_avl[power_mode][i].std_level; + + switch (max_odr) { + case 0: + sdata->cdata->accel_deltatime = 0; + break; + + case 12: + sdata->cdata->accel_deltatime = + 80000000LL * sdata->cdata->hwfifo_watermark; + break; + + default: + sdata->cdata->accel_deltatime = + div_s64(1000000000LL, max_odr) * + sdata->cdata->hwfifo_watermark; + break; + } + + return 0; +} + +int ism303dac_update_drdy_irq(struct ism303dac_sensor_data *sdata, bool state) +{ + u8 reg_addr, reg_val, reg_mask; + + switch (sdata->sindex) { + case ISM303DAC_TAP: + case ISM303DAC_DOUBLE_TAP: + reg_val = ism303dac_event_irq1_value(sdata->cdata); + reg_addr = ISM303DAC_CTRL4_INT1_PAD_ADDR; + reg_mask = ISM303DAC_INT1_EVENTS_MASK; + + break; + + case ISM303DAC_ACCEL: + reg_addr = ISM303DAC_CTRL4_INT1_PAD_ADDR; + reg_mask = (sdata->cdata->hwfifo_enabled) ? + ISM303DAC_INT1_FTH_MASK: + ISM303DAC_DRDY_MASK; + if (state) + reg_val = ISM303DAC_EN_BIT; + else + reg_val = ISM303DAC_DIS_BIT; + + break; + + default: + return -EINVAL; + } + + return ism303dac_write_register(sdata->cdata, reg_addr, reg_mask, + reg_val, true); +} + +static int ism303dac_update_fifo(struct ism303dac_data *cdata, u16 watermark) +{ + int err; + int fifo_size; + struct iio_dev *indio_dev; + + indio_dev = cdata->iio_sensors_dev[ISM303DAC_ACCEL]; + cdata->timestamp = ism303dac_get_time_ns(indio_dev); + cdata->sample_timestamp = cdata->timestamp; + cdata->samples = 0; + + err = ism303dac_write_register(cdata, ISM303DAC_FIFO_THS_ADDR, + ISM303DAC_FIFO_THS_MASK, + watermark, true); + if (err < 0) + return err; + + if (cdata->fifo_data) + kfree(cdata->fifo_data); + + cdata->fifo_data = 0; + + fifo_size = watermark * ISM303DAC_FIFO_BYTE_FOR_SAMPLE; + if (fifo_size > 0) { + cdata->fifo_data = kmalloc(fifo_size, GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + + cdata->fifo_size = fifo_size; + } + + return ism303dac_set_fifo_mode(cdata, CONTINUOS); +} + +int ism303dac_set_enable(struct ism303dac_sensor_data *sdata, bool state) +{ + int err = 0; + u8 mode; + + if (sdata->enabled == state) + return 0; + + /* + * Start assuming the sensor enabled if state == true. + * It will be restored if an error occur. + */ + if (state) { + SET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + mode = CONTINUOS; + } else { + RESET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + mode = BYPASS; + } + + switch (sdata->sindex) { + case ISM303DAC_TAP: + if (state && CHECK_BIT(sdata->cdata->enabled_sensor, + ISM303DAC_DOUBLE_TAP)) { + err = -EINVAL; + + goto enable_sensor_error; + } + + break; + + case ISM303DAC_DOUBLE_TAP: + if (state && CHECK_BIT(sdata->cdata->enabled_sensor, + ISM303DAC_TAP)) { + err = -EINVAL; + + goto enable_sensor_error; + } + + break; + + case ISM303DAC_ACCEL: + break; + + default: + return -EINVAL; + } + + err = ism303dac_update_drdy_irq(sdata, state); + if (err < 0) + goto enable_sensor_error; + + err = ism303dac_set_fifo_mode(sdata->cdata, mode); + if (err < 0) + return err; + + err = ism303dac_write_max_odr(sdata); + if (err < 0) + goto enable_sensor_error; + + sdata->enabled = state; + + return 0; + +enable_sensor_error: + if (state) { + RESET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + } else { + SET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + } + + return err; +} + +static int ism303dac_init_sensors(struct ism303dac_data *cdata) +{ + int err, i; + struct ism303dac_sensor_data *sdata; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) { + sdata = iio_priv(cdata->iio_sensors_dev[i]); + + err = ism303dac_set_enable(sdata, false); + if (err < 0) + return err; + + if (sdata->sindex == ISM303DAC_ACCEL) { + err = ism303dac_set_fs(sdata, ISM303DAC_DEFAULT_ACCEL_FS); + if (err < 0) + return err; + } + } + + cdata->selftest_status = 0; + + /* + * Soft reset the device on power on. + */ + err = ism303dac_write_register(cdata, ISM303DAC_SOFT_RESET_ADDR, + ISM303DAC_SOFT_RESET_MASK, + ISM303DAC_EN_BIT, true); + if (err < 0) + return err; + + if (cdata->spi_3wire) { + u8 data = ISM303DAC_ADD_INC_MASK | ISM303DAC_SIM_MASK; + + err = cdata->tf->write(cdata, ISM303DAC_SIM_ADDR, 1, &data, + false); + if (err < 0) + return err; + } + + /* + * Enable latched interrupt mode. + */ + err = ism303dac_write_register(cdata, ISM303DAC_LIR_ADDR, + ISM303DAC_LIR_MASK, + ISM303DAC_EN_BIT, true); + if (err < 0) + return err; + + /* + * Enable block data update feature. + */ + err = ism303dac_write_register(cdata, ISM303DAC_BDU_ADDR, + ISM303DAC_BDU_MASK, + ISM303DAC_EN_BIT, true); + if (err < 0) + return err; + + /* + * Route interrupt from INT2 to INT1 pin. + */ + err = ism303dac_write_register(cdata, ISM303DAC_INT2_ON_INT1_ADDR, + ISM303DAC_INT2_ON_INT1_MASK, + ISM303DAC_EN_BIT, true); + if (err < 0) + return err; + + /* + * Configure default free fall event threshold. + */ + err = ism303dac_write_register(sdata->cdata, ISM303DAC_FREE_FALL_ADDR, + ISM303DAC_FREE_FALL_THS_MASK, + ISM303DAC_FREE_FALL_THS_DEFAULT, true); + if (err < 0) + return err; + + /* + * Configure default free fall event duration. + */ + err = ism303dac_write_register(sdata->cdata, ISM303DAC_FREE_FALL_ADDR, + ISM303DAC_FREE_FALL_DUR_MASK, + ISM303DAC_FREE_FALL_DUR_DEFAULT, true); + if (err < 0) + return err; + + /* + * Configure Tap event recognition on all direction (X, Y and Z axes). + */ + err = ism303dac_write_register(sdata->cdata, ISM303DAC_TAP_AXIS_ADDR, + ISM303DAC_TAP_AXIS_MASK, + ISM303DAC_TAP_AXIS_ANABLE_ALL, true); + if (err < 0) + return err; + + /* + * Configure default threshold for Tap event recognition. + */ + err = ism303dac_write_register(sdata->cdata, ISM303DAC_TAP_THS_ADDR, + ISM303DAC_TAP_THS_MASK, + ISM303DAC_TAP_THS_DEFAULT, true); + if (err < 0) + return err; + + /* + * Configure default threshold for Wake Up event recognition. + */ + err = ism303dac_write_register(sdata->cdata, ISM303DAC_WAKE_UP_THS_ADDR, + ISM303DAC_WAKE_UP_THS_WU_MASK, + ISM303DAC_WAKE_UP_THS_WU_DEFAULT, true); + if (err < 0) + return err; + + return 0; +} + +static ssize_t ism303dac_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct ism303dac_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->odr); +} + +ssize_t ism303dac_set_sampling_frequency(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + int err; + u8 power_mode; + unsigned int odr, i; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + if (sdata->odr == odr) + return count; + + power_mode = sdata->cdata->power_mode; + for (i = 0; i < ISM303DAC_ODR_LP_LIST_NUM; i++) { + if (ism303dac_odr_table.odr_avl[power_mode][i].hz >= odr) + break; + } + if (i == ISM303DAC_ODR_LP_LIST_NUM) + return -EINVAL; + + mutex_lock(&indio_dev->mlock); + sdata->odr = ism303dac_odr_table.odr_avl[power_mode][i].hz; + mutex_unlock(&indio_dev->mlock); + + err = ism303dac_write_max_odr(sdata); + + return (err < 0) ? err : count; +} + +static ssize_t ism303dac_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0, mode_count, mode; + struct ism303dac_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + mode = sdata->cdata->power_mode; + mode_count = (mode == ISM303DAC_LP_MODE) ? + ISM303DAC_ODR_LP_LIST_NUM : ISM303DAC_ODR_HR_LIST_NUM; + + for (i = 1; i < mode_count; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + ism303dac_odr_table.odr_avl[mode][i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t ism303dac_get_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ISM303DAC_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + ism303dac_fs_table.fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static int ism303dac_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[2]; + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = ism303dac_set_enable(sdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + msleep(40); + + err = ism303dac_read_register(sdata->cdata, ch->address, 2, + outdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + err = ism303dac_set_enable(sdata, false); + mutex_unlock(&indio_dev->mlock); + + if (err < 0) + return err; + + return IIO_VAL_INT; + + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->gain; + + return IIO_VAL_INT_PLUS_MICRO; + + default: + return -EINVAL; + } + + return 0; +} + +static int ism303dac_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + int err, i; + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + for (i = 0; i < ISM303DAC_FS_LIST_NUM; i++) { + if (ism303dac_fs_table.fs_avl[i].gain == val2) + break; + } + + err = ism303dac_set_fs(sdata, ism303dac_fs_table.fs_avl[i].urv); + mutex_unlock(&indio_dev->mlock); + + break; + + default: + return -EINVAL; + } + + return err; +} + +static ssize_t ism303dac_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->hwfifo_enabled); +} + +ssize_t ism303dac_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + int err = 0, enable = 0; + u8 mode = BYPASS; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &enable); + if (err < 0) + return err; + + if (enable != 0x0 && enable != 0x1) + return -EINVAL; + + mode = (enable == 0x0) ? BYPASS : CONTINUOS; + + err = ism303dac_set_fifo_mode(sdata->cdata, mode); + if (err < 0) + return err; + + sdata->cdata->hwfifo_enabled = enable; + + return count; +} + +static ssize_t ism303dac_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->hwfifo_watermark); +} + +ssize_t ism303dac_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + int err = 0, watermark = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > ISM303DAC_MAX_FIFO_THS)) + return -EINVAL; + + mutex_lock(&sdata->cdata->fifo_lock); + err = ism303dac_update_fifo(sdata->cdata, watermark); + mutex_unlock(&sdata->cdata->fifo_lock); + if (err < 0) + return err; + + sdata->cdata->hwfifo_watermark = watermark; + + return count; +} + +static ssize_t ism303dac_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +static ssize_t ism303dac_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ISM303DAC_MAX_FIFO_THS); +} + +ssize_t ism303dac_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + u64 event_type; + int64_t sensor_last_timestamp; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = ism303dac_get_time_ns(indio_dev); + + mutex_lock(&sdata->cdata->fifo_lock); + ism303dac_read_fifo(sdata->cdata, true); + mutex_unlock(&sdata->cdata->fifo_lock); + + sdata->cdata->timestamp = sensor_last_timestamp; + + if (sensor_last_timestamp == sdata->cdata->sample_timestamp) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(IIO_ACCEL, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + sensor_last_timestamp); + + enable_irq(sdata->cdata->irq); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static ssize_t ism303dac_get_selftest_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%s %s %s\n", ism303dac_selftest_table[0].mode_str, + ism303dac_selftest_table[1].mode_str, + ism303dac_selftest_table[2].mode_str); +} + +static ssize_t ism303dac_get_selftest_status(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + u8 status; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + status = sdata->cdata->selftest_status; + return sprintf(buf, "%s\n", ism303dac_selftest_table[status].mode_str); +} + +static ssize_t ism303dac_set_selftest_status(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + int err, i; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism303dac_sensor_data *sdata = iio_priv(indio_dev); + + for (i = 0; i < ARRAY_SIZE(ism303dac_selftest_table); i++) { + if (strncmp(buf, ism303dac_selftest_table[i].mode_str, + size - 2) == 0) + break; + } + if (i == ARRAY_SIZE(ism303dac_selftest_table)) + return -EINVAL; + + err = ism303dac_set_selftest_mode(sdata, i); + if (err < 0) + return err; + + sdata->cdata->selftest_status = i; + + return size; +} + +static ST_ISM303DAC_HWFIFO_ENABLED(); +static ST_ISM303DAC_HWFIFO_WATERMARK(); +static ST_ISM303DAC_HWFIFO_WATERMARK_MIN(); +static ST_ISM303DAC_HWFIFO_WATERMARK_MAX(); +static ST_ISM303DAC_HWFIFO_FLUSH(); + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + ism303dac_get_sampling_frequency, + ism303dac_set_sampling_frequency); +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(ism303dac_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO, + ism303dac_get_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + ism303dac_get_selftest_avail, NULL, 0); +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + ism303dac_get_selftest_status, + ism303dac_set_selftest_status, 0); + +static struct attribute *ism303dac_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + + NULL, +}; + +static struct attribute *ism303dac_step_tap_attributes[] = { + NULL, +}; + +static struct attribute *ism303dac_step_double_tap_attributes[] = { + NULL, +}; + +static const struct attribute_group ism303dac_accel_attribute_group = { + .attrs = ism303dac_accel_attributes, +}; + +static const struct attribute_group ism303dac_tap_attribute_group = { + .attrs = ism303dac_step_tap_attributes, +}; + +static const struct attribute_group ism303dac_double_tap_attribute_group = { + .attrs = ism303dac_step_double_tap_attributes, +}; + +static const struct iio_info ism303dac_info[ISM303DAC_SENSORS_NUMB] = { + [ISM303DAC_ACCEL] = { + .attrs = &ism303dac_accel_attribute_group, + .read_raw = &ism303dac_read_raw, + .write_raw = &ism303dac_write_raw, + }, + [ISM303DAC_TAP] = { + .attrs = &ism303dac_tap_attribute_group, + }, + [ISM303DAC_DOUBLE_TAP] = { + .attrs = &ism303dac_double_tap_attribute_group, + }, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops ism303dac_trigger_ops = { + .set_trigger_state = (&ism303dac_trig_set_state), +}; +#define ISM303DAC_TRIGGER_OPS (&ism303dac_trigger_ops) +#else +#define ISM303DAC_TRIGGER_OPS NULL +#endif + +#ifdef CONFIG_OF +static u32 ism303dac_parse_dt(struct ism303dac_data *cdata) +{ + u32 val; + struct device_node *np; + + np = cdata->dev->of_node; + if (!np) + return -EINVAL; + + if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && + (val <= 2) && (val > 0)) + cdata->drdy_int_pin = (u8) val; + else + cdata->drdy_int_pin = 1; + + return 0; +} +#endif + +static int ism303dac_init_interface(struct ism303dac_data *cdata) +{ + struct device_node *np = cdata->dev->of_node; + + if (np && of_property_read_bool(np, "spi-3wire")) { + u8 data; + int err; + + data = ISM303DAC_ADD_INC_MASK | ISM303DAC_SIM_MASK; + err = cdata->tf->write(cdata, ISM303DAC_SIM_ADDR, 1, &data, + false); + if (err < 0) + return err; + + cdata->spi_3wire = true; + } + + return 0; +} + +int ism303dac_common_probe(struct ism303dac_data *cdata, int irq) +{ + u8 wai = 0; + int32_t err, i, n; + struct iio_dev *piio_dev; + struct ism303dac_sensor_data *sdata; + + mutex_init(&cdata->regs_lock); + mutex_init(&cdata->tb.buf_lock); + mutex_init(&cdata->fifo_lock); + + cdata->fifo_data = 0; + + err = ism303dac_init_interface(cdata); + if (err < 0) + return err; + + err = ism303dac_read_register(cdata, ISM303DAC_WHO_AM_I_ADDR, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + if (wai != ISM303DAC_WHO_AM_I_DEF) { + dev_err(cdata->dev, "Who-Am-I value not valid (%x).\n", wai); + + return -ENODEV; + } + + cdata->hwfifo_enabled = 0; + + err = ism303dac_set_fifo_mode(cdata, BYPASS); + if (err < 0) + return err; + + if (irq > 0) { + cdata->irq = irq; +#ifdef CONFIG_OF + err = ism303dac_parse_dt(cdata); + if (err < 0) + return err; +#else /* CONFIG_OF */ + if (cdata->dev->platform_data) { + cdata->drdy_int_pin = ((struct ism303dac_platform_data *) + cdata->dev->platform_data)->drdy_int_pin; + + if ((cdata->drdy_int_pin > 2) || + (cdata->drdy_int_pin < 1)) + cdata->drdy_int_pin = 1; + } else + cdata->drdy_int_pin = 1; +#endif /* CONFIG_OF */ + + dev_info(cdata->dev, "driver use DRDY int pin %d\n", + cdata->drdy_int_pin); + } + + cdata->common_odr = 0; + cdata->enabled_sensor = 0; + /* Set min watermark. */ + cdata->hwfifo_watermark = 1; + + /* + * Select sensor power mode operation. + * + * - ISM303DAC_LP_MODE: Low Power. The output data are 10 bits encoded. + * - ISM303DAC_HR_MODE: High Resolution. 14 bits output data encoding. + */ + cdata->power_mode = ISM303DAC_MODE_DEFAULT; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) { + piio_dev = devm_iio_device_alloc(cdata->dev, + sizeof(struct ism303dac_sensor_data *)); + if (!piio_dev) + return -ENOMEM; + + cdata->iio_sensors_dev[i] = piio_dev; + sdata = iio_priv(piio_dev); + sdata->enabled = false; + sdata->cdata = cdata; + sdata->sindex = i; + sdata->name = ism303dac_sensors_table[i].name; + sdata->odr = ism303dac_sensors_table[i].min_odr_hz; + + piio_dev->channels = ism303dac_sensors_table[i].iio_channel; + piio_dev->num_channels = + ism303dac_sensors_table[i].iio_channel_size; + piio_dev->info = &ism303dac_info[i]; + piio_dev->modes = INDIO_DIRECT_MODE; + piio_dev->name = kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + sdata->name); + } + + err = ism303dac_init_sensors(cdata); + if (err < 0) + return err; + + err = ism303dac_allocate_rings(cdata); + if (err < 0) + return err; + + if (irq > 0) { + err = ism303dac_allocate_triggers(cdata, ISM303DAC_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < ISM303DAC_SENSORS_NUMB; n++) { + err = iio_device_register(cdata->iio_sensors_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + dev_info(cdata->dev, "%s: probed\n", ISM303DAC_DEV_NAME); + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->iio_sensors_dev[n]); + +deallocate_ring: + ism303dac_deallocate_rings(cdata); + return err; +} +EXPORT_SYMBOL(ism303dac_common_probe); + +void ism303dac_common_remove(struct ism303dac_data *cdata, int irq) +{ + int i; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) + iio_device_unregister(cdata->iio_sensors_dev[i]); + + if (irq > 0) + ism303dac_deallocate_triggers(cdata); + + ism303dac_deallocate_rings(cdata); +} +EXPORT_SYMBOL(ism303dac_common_remove); + +#ifdef CONFIG_PM +int __maybe_unused ism303dac_common_suspend(struct ism303dac_data *cdata) +{ + return 0; +} +EXPORT_SYMBOL(ism303dac_common_suspend); + +int __maybe_unused ism303dac_common_resume(struct ism303dac_data *cdata) +{ + return 0; +} +EXPORT_SYMBOL(ism303dac_common_resume); +#endif /* CONFIG_PM */ + +MODULE_DESCRIPTION("STMicroelectronics ism303dac core driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_ism303dac_accel_i2c.c b/drivers/iio/stm/accel/st_ism303dac_accel_i2c.c new file mode 100644 index 000000000000..4edeb60133e6 --- /dev/null +++ b/drivers/iio/stm/accel/st_ism303dac_accel_i2c.c @@ -0,0 +1,174 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism303dac i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2018 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_ism303dac_accel.h" + +static int ism303dac_i2c_read(struct ism303dac_data *cdata, u8 reg_addr, int len, + u8 * data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->regs_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->regs_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int ism303dac_i2c_write(struct ism303dac_data *cdata, u8 reg_addr, int len, + u8 * data, bool b_lock) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + int err = 0; + u8 send[8]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + if (b_lock) { + mutex_lock(&cdata->regs_lock); + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->regs_lock); + } else + err = i2c_transfer(client->adapter, &msg, 1); + + return err; +} + +static const struct ism303dac_transfer_function ism303dac_tf_i2c = { + .write = ism303dac_i2c_write, + .read = ism303dac_i2c_read, +}; + +static int ism303dac_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct ism303dac_data *cdata; + + cdata = kzalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + cdata->tf = &ism303dac_tf_i2c; + i2c_set_clientdata(client, cdata); + + err = ism303dac_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int ism303dac_i2c_remove(struct i2c_client *client) +{ + struct ism303dac_data *cdata = i2c_get_clientdata(client); + + ism303dac_common_remove(cdata, client->irq); + dev_info(cdata->dev, "%s: removed\n", ISM303DAC_DEV_NAME); + kfree(cdata); + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused ism303dac_suspend(struct device *dev) +{ + struct ism303dac_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return ism303dac_common_suspend(cdata); +} + +static int __maybe_unused ism303dac_resume(struct device *dev) +{ + struct ism303dac_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return ism303dac_common_resume(cdata); +} + +static const struct dev_pm_ops ism303dac_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(ism303dac_suspend, ism303dac_resume) +}; + +#define ISM303DAC_PM_OPS (&ism303dac_pm_ops) +#else /* CONFIG_PM */ +#define ISM303DAC_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id ism303dac_ids[] = { + { ISM303DAC_DEV_NAME, 0 }, + {} +}; + +MODULE_DEVICE_TABLE(i2c, ism303dac_ids); + +#ifdef CONFIG_OF +static const struct of_device_id ism303dac_id_table[] = { + {.compatible = "st,ism303dac_accel",}, + {}, +}; + +MODULE_DEVICE_TABLE(of, ism303dac_id_table); +#endif + +static struct i2c_driver ism303dac_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = ISM303DAC_DEV_NAME, + .pm = ISM303DAC_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = ism303dac_id_table, +#endif + }, + .probe = ism303dac_i2c_probe, + .remove = ism303dac_i2c_remove, + .id_table = ism303dac_ids, +}; + +module_i2c_driver(ism303dac_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics ism303dac i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_ism303dac_accel_spi.c b/drivers/iio/stm/accel/st_ism303dac_accel_spi.c new file mode 100644 index 000000000000..5ac54c469acf --- /dev/null +++ b/drivers/iio/stm/accel/st_ism303dac_accel_spi.c @@ -0,0 +1,192 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism303dac spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2018 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_ism303dac_accel.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int ism303dac_spi_read(struct ism303dac_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->regs_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return err; +} + +static int ism303dac_spi_write(struct ism303dac_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ISM303DAC_TX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->regs_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return err; +} + +static const struct ism303dac_transfer_function ism303dac_tf_spi = { + .write = ism303dac_spi_write, + .read = ism303dac_spi_read, +}; + +static int ism303dac_spi_probe(struct spi_device *spi) +{ + int err; + struct ism303dac_data *cdata; + + cdata = kzalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + cdata->tf = &ism303dac_tf_spi; + spi_set_drvdata(spi, cdata); + + err = ism303dac_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int ism303dac_spi_remove(struct spi_device *spi) +{ + struct ism303dac_data *cdata = spi_get_drvdata(spi); + + ism303dac_common_remove(cdata, spi->irq); + dev_info(cdata->dev, "%s: removed\n", ISM303DAC_DEV_NAME); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused ism303dac_suspend(struct device *dev) +{ + struct ism303dac_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return ism303dac_common_suspend(cdata); +} + +static int __maybe_unused ism303dac_resume(struct device *dev) +{ + struct ism303dac_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return ism303dac_common_resume(cdata); +} + +static const struct dev_pm_ops ism303dac_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(ism303dac_suspend, ism303dac_resume) +}; + +#define ISM303DAC_PM_OPS (&ism303dac_pm_ops) +#else /* CONFIG_PM */ +#define ISM303DAC_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id ism303dac_ids[] = { + { ISM303DAC_DEV_NAME, 0 }, + {} +}; + +MODULE_DEVICE_TABLE(spi, ism303dac_ids); + +#ifdef CONFIG_OF +static const struct of_device_id ism303dac_id_table[] = { + {.compatible = "st,ism303dac_accel",}, + {}, +}; + +MODULE_DEVICE_TABLE(of, ism303dac_id_table); +#endif /* CONFIG_OF */ + +static struct spi_driver ism303dac_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = ISM303DAC_DEV_NAME, + .pm = ISM303DAC_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = ism303dac_id_table, +#endif /* CONFIG_OF */ + }, + .probe = ism303dac_spi_probe, + .remove = ism303dac_spi_remove, + .id_table = ism303dac_ids, +}; + +module_spi_driver(ism303dac_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics ism303dac spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_ism303dac_accel_trigger.c b/drivers/iio/stm/accel/st_ism303dac_accel_trigger.c new file mode 100644 index 000000000000..e4c475d6ad7f --- /dev/null +++ b/drivers/iio/stm/accel/st_ism303dac_accel_trigger.c @@ -0,0 +1,166 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism303dac driver + * + * MEMS Software Solutions Team + * + * Copyright 2018 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_ism303dac_accel.h" + +static void ism303dac_event_management(struct ism303dac_data *cdata, + u8 int_reg_val) +{ + u8 status; + + /* Must read TAP_SRC to remove irq bits */ + cdata->tf->read(cdata, ISM303DAC_TAP_SRC_ADDR, 1, &status, true); + + if (CHECK_BIT(cdata->enabled_sensor, ISM303DAC_TAP) && + (int_reg_val & ISM303DAC_TAP_MASK)) + iio_push_event(cdata->iio_sensors_dev[ISM303DAC_TAP], + IIO_UNMOD_EVENT_CODE(IIO_TAP, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); + + if (CHECK_BIT(cdata->enabled_sensor, ISM303DAC_DOUBLE_TAP) && + (int_reg_val & ISM303DAC_DOUBLE_TAP_MASK)) + iio_push_event(cdata->iio_sensors_dev[ISM303DAC_DOUBLE_TAP], + IIO_UNMOD_EVENT_CODE(IIO_TAP_TAP, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); +} + +static inline s64 st_ism303dac_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ISM303DAC_EWMA_DIV - weight) * + diff, ISM303DAC_EWMA_DIV); + + return old + incr; +} + +static irqreturn_t ism303dac_irq_handler(int irq, void *private) +{ + u8 ewma_level; + struct ism303dac_data *cdata = private; + s64 ts; + + ewma_level = (cdata->common_odr >= 100) ? 120 : 96; + ts = ism303dac_get_time_ns(cdata->iio_sensors_dev[ISM303DAC_ACCEL]); + cdata->accel_deltatime = st_ism303dac_ewma(cdata->accel_deltatime, + ts - cdata->timestamp, + ewma_level); + cdata->timestamp = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t ism303dac_irq_thread(int irq, void *private) +{ + u8 status; + struct ism303dac_data *cdata = private; + + if (CHECK_BIT(cdata->enabled_sensor, ISM303DAC_ACCEL)) { + if (cdata->hwfifo_enabled) { + mutex_lock(&cdata->fifo_lock); + ism303dac_read_fifo(cdata, true); + mutex_unlock(&cdata->fifo_lock); + } else { + cdata->tf->read(cdata, ISM303DAC_STATUS_DUP_ADDR, 1, &status, true); + if (status & (ISM303DAC_DRDY_MASK)) + ism303dac_read_xyz(cdata); + } + } + + if (cdata->enabled_sensor & ~(1 << ISM303DAC_ACCEL)) { + cdata->tf->read(cdata, ISM303DAC_STATUS_DUP_ADDR, 1, &status, true); + if (status & ISM303DAC_EVENT_MASK) + ism303dac_event_management(cdata, status); + } + + return IRQ_HANDLED; +} + +int ism303dac_allocate_triggers(struct ism303dac_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) { +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->iio_trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->iio_sensors_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->iio_trig[i] = iio_trigger_alloc("%s-trigger", + cdata->iio_sensors_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->iio_trig[i]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + err = -ENOMEM; + + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->iio_trig[i], + cdata->iio_sensors_dev[i]); + cdata->iio_trig[i]->ops = trigger_ops; + cdata->iio_trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, + ism303dac_irq_handler, + ism303dac_irq_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < ISM303DAC_SENSORS_NUMB; n++) { + err = iio_trigger_register(cdata->iio_trig[n]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + + goto free_irq; + } + + cdata->iio_sensors_dev[n]->trig = cdata->iio_trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->iio_trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->iio_trig[i]); + + return err; +} + +void ism303dac_deallocate_triggers(struct ism303dac_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < ISM303DAC_SENSORS_NUMB; i++) + iio_trigger_unregister(cdata->iio_trig[i]); +} diff --git a/drivers/iio/stm/accel/st_lis2ds12.h b/drivers/iio/stm/accel/st_lis2ds12.h new file mode 100644 index 000000000000..d55d7d377b54 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2ds12.h @@ -0,0 +1,336 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lis2ds12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2015 STMicroelectronics Inc. + */ + +#ifndef __LIS2DS12_H +#define __LIS2DS12_H + +#include +#include +#include +#include + +#define LIS2DS12_WHO_AM_I_ADDR 0x0f +#define LIS2DS12_WHO_AM_I_DEF 0x43 +#define LIS2DS12_CTRL1_ADDR 0x20 +#define LIS2DS12_CTRL2_ADDR 0x21 +#define LIS2DS12_CTRL3_ADDR 0x22 +#define LIS2DS12_CTRL4_INT1_PAD_ADDR 0x23 +#define LIS2DS12_CTRL5_INT2_PAD_ADDR 0x24 +#define LIS2DS12_FIFO_CTRL_ADDR 0x25 +#define LIS2DS12_OUTX_L_ADDR 0x28 +#define LIS2DS12_OUTY_L_ADDR 0x2a +#define LIS2DS12_OUTZ_L_ADDR 0x2c +#define LIS2DS12_TAP_THS_6D_ADDR 0x31 +#define LIS2DS12_WAKE_UP_THS_ADDR 0x33 +#define LIS2DS12_FREE_FALL_ADDR 0x35 +#define LIS2DS12_STEP_C_MINTHS_ADDR 0x3a +#define LIS2DS12_STEP_C_MINTHS_RST_NSTEP_MASK 0x80 +#define LIS2DS12_STEP_C_OUT_L_ADDR 0x3b +#define LIS2DS12_FUNC_CTRL_ADDR 0x3f +#define LIS2DS12_FUNC_CTRL_TILT_MASK 0x10 +#define LIS2DS12_FUNC_CTRL_SIGN_MOT_MASK 0x02 +#define LIS2DS12_FUNC_CTRL_STEP_CNT_MASK 0x01 +#define LIS2DS12_FUNC_CTRL_EV_MASK (LIS2DS12_FUNC_CTRL_TILT_MASK | \ + LIS2DS12_FUNC_CTRL_SIGN_MOT_MASK | \ + LIS2DS12_FUNC_CTRL_STEP_CNT_MASK) +#define LIS2DS12_FIFO_THS_ADDR 0x2e +#define LIS2DS12_FIFO_THS_MASK 0xff +#define LIS2DS12_ODR_ADDR LIS2DS12_CTRL1_ADDR +#define LIS2DS12_ODR_MASK 0xf0 +#define LIS2DS12_ODR_POWER_OFF_VAL 0x00 +#define LIS2DS12_ODR_1HZ_LP_VAL 0x08 +#define LIS2DS12_ODR_12HZ_LP_VAL 0x09 +#define LIS2DS12_ODR_25HZ_LP_VAL 0x0a +#define LIS2DS12_ODR_50HZ_LP_VAL 0x0b +#define LIS2DS12_ODR_100HZ_LP_VAL 0x0c +#define LIS2DS12_ODR_200HZ_LP_VAL 0x0d +#define LIS2DS12_ODR_400HZ_LP_VAL 0x0e +#define LIS2DS12_ODR_800HZ_LP_VAL 0x0f +#define LIS2DS12_ODR_LP_LIST_NUM 9 + +#define LIS2DS12_ODR_12_5HZ_HR_VAL 0x01 +#define LIS2DS12_ODR_25HZ_HR_VAL 0x02 +#define LIS2DS12_ODR_50HZ_HR_VAL 0x03 +#define LIS2DS12_ODR_100HZ_HR_VAL 0x04 +#define LIS2DS12_ODR_200HZ_HR_VAL 0x05 +#define LIS2DS12_ODR_400HZ_HR_VAL 0x06 +#define LIS2DS12_ODR_800HZ_HR_VAL 0x07 +#define LIS2DS12_ODR_HR_LIST_NUM 8 + +#define LIS2DS12_FS_ADDR LIS2DS12_CTRL1_ADDR +#define LIS2DS12_FS_MASK 0x0c +#define LIS2DS12_FS_2G_VAL 0x00 +#define LIS2DS12_FS_4G_VAL 0x02 +#define LIS2DS12_FS_8G_VAL 0x03 +#define LIS2DS12_FS_16G_VAL 0x01 + +/* Advanced Configuration Registers */ +#define LIS2DS12_FUNC_CFG_ENTER_ADDR LIS2DS12_CTRL2_ADDR +#define LIS2DS12_FUNC_CFG_EXIT_ADDR 0x3F +#define LIS2DS12_FUNC_CFG_EN_MASK 0x10 +#define LIS2DS12_STEP_COUNT_DELTA 0x3A + +#define LIS2DS12_SIM_ADDR LIS2DS12_CTRL2_ADDR +#define LIS2DS12_SIM_MASK 0x01 +#define LIS2DS12_ADD_INC_MASK 0x04 + +/* + * Sensitivity for the 16-bit data + */ +#define LIS2DS12_FS_2G_GAIN IIO_G_TO_M_S_2(61) +#define LIS2DS12_FS_4G_GAIN IIO_G_TO_M_S_2(122) +#define LIS2DS12_FS_8G_GAIN IIO_G_TO_M_S_2(244) +#define LIS2DS12_FS_16G_GAIN IIO_G_TO_M_S_2(488) + +#define LIS2DS12_MODE_DEFAULT LIS2DS12_HR_MODE +#define LIS2DS12_INT1_S_TAP_MASK 0x40 +#define LIS2DS12_INT1_WAKEUP_MASK 0x20 +#define LIS2DS12_INT1_FREE_FALL_MASK 0x10 +#define LIS2DS12_INT1_TAP_MASK 0x08 +#define LIS2DS12_INT1_6D_MASK 0x04 +#define LIS2DS12_INT1_FTH_MASK 0x02 +#define LIS2DS12_INT1_DRDY_MASK 0x01 +#define LIS2DS12_INT1_EVENTS_MASK (LIS2DS12_INT1_S_TAP_MASK | \ + LIS2DS12_INT1_WAKEUP_MASK | \ + LIS2DS12_INT1_FREE_FALL_MASK | \ + LIS2DS12_INT1_TAP_MASK | \ + LIS2DS12_INT1_6D_MASK | \ + LIS2DS12_INT1_FTH_MASK | \ + LIS2DS12_INT1_DRDY_MASK) +#define LIS2DS12_INT2_ON_INT1_MASK 0x20 +#define LIS2DS12_INT2_TILT_MASK 0x10 +#define LIS2DS12_INT2_SIG_MOT_DET_MASK 0x08 +#define LIS2DS12_INT2_STEP_DET_MASK 0x04 +#define LIS2DS12_INT2_FTH_MASK 0x02 +#define LIS2DS12_INT2_DRDY_MASK 0x01 +#define LIS2DS12_INT2_EVENTS_MASK (LIS2DS12_INT2_TILT_MASK | \ + LIS2DS12_INT2_SIG_MOT_DET_MASK | \ + LIS2DS12_INT2_STEP_DET_MASK | \ + LIS2DS12_INT2_FTH_MASK | \ + LIS2DS12_INT2_DRDY_MASK) +#define LIS2DS12_WAKE_UP_THS_WU_MASK 0x3f +#define LIS2DS12_WAKE_UP_THS_WU_DEFAULT 0x02 +#define LIS2DS12_FREE_FALL_THS_MASK 0x07 +#define LIS2DS12_FREE_FALL_DUR_MASK 0xF8 +#define LIS2DS12_FREE_FALL_THS_DEFAULT 0x01 +#define LIS2DS12_FREE_FALL_DUR_DEFAULT 0x01 +#define LIS2DS12_BDU_ADDR LIS2DS12_CTRL1_ADDR +#define LIS2DS12_BDU_MASK 0x01 +#define LIS2DS12_SOFT_RESET_ADDR LIS2DS12_CTRL2_ADDR +#define LIS2DS12_SOFT_RESET_MASK 0x40 +#define LIS2DS12_LIR_ADDR LIS2DS12_CTRL3_ADDR +#define LIS2DS12_LIR_MASK 0x04 +#define LIS2DS12_TAP_AXIS_ADDR LIS2DS12_CTRL3_ADDR +#define LIS2DS12_TAP_AXIS_MASK 0x38 +#define LIS2DS12_TAP_AXIS_ANABLE_ALL 0x07 +#define LIS2DS12_TAP_THS_ADDR LIS2DS12_TAP_THS_6D_ADDR +#define LIS2DS12_TAP_THS_MASK 0x1f +#define LIS2DS12_TAP_THS_DEFAULT 0x09 +#define LIS2DS12_INT2_ON_INT1_ADDR LIS2DS12_CTRL5_INT2_PAD_ADDR +#define LIS2DS12_INT2_ON_INT1_MASK 0x20 +#define LIS2DS12_FIFO_MODE_ADDR LIS2DS12_FIFO_CTRL_ADDR +#define LIS2DS12_FIFO_MODE_MASK 0xe0 +#define LIS2DS12_FIFO_MODE_BYPASS 0x00 +#define LIS2DS12_FIFO_MODE_CONTINUOS 0x06 +#define LIS2DS12_OUT_XYZ_SIZE 8 + +#define LIS2DS12_SELFTEST_ADDR LIS2DS12_CTRL3_ADDR +#define LIS2DS12_SELFTEST_MASK 0xc0 +#define LIS2DS12_SELFTEST_NORMAL 0x00 +#define LIS2DS12_SELFTEST_POS_SIGN 0x01 +#define LIS2DS12_SELFTEST_NEG_SIGN 0x02 + +#define LIS2DS12_FIFO_SRC 0x2f +#define LIS2DS12_FIFO_SRC_DIFF_MASK 0x20 + +#define LIS2DS12_FIFO_NUM_AXIS 3 +#define LIS2DS12_FIFO_BYTE_X_AXIS 2 +#define LIS2DS12_FIFO_BYTE_FOR_SAMPLE (LIS2DS12_FIFO_NUM_AXIS * \ + LIS2DS12_FIFO_BYTE_X_AXIS) +#define LIS2DS12_TIMESTAMP_SIZE 8 + +#define LIS2DS12_STATUS_ADDR 0x27 +#define LIS2DS12_STATUS_DUP_ADDR 0x36 +#define LIS2DS12_FUNC_CK_GATE_ADDR 0x3d +#define LIS2DS12_FUNC_CK_GATE_TILT_INT_MASK 0x80 +#define LIS2DS12_FUNC_CK_GATE_SIGN_M_DET_MASK 0x10 +#define LIS2DS12_FUNC_CK_GATE_RST_SIGN_M_MASK 0x08 +#define LIS2DS12_FUNC_CK_GATE_RST_PEDO_MASK 0x04 +#define LIS2DS12_FUNC_CK_GATE_STEP_D_MASK 0x02 +#define LIS2DS12_FUNC_CK_GATE_MASK (LIS2DS12_FUNC_CK_GATE_TILT_INT_MASK | \ + LIS2DS12_FUNC_CK_GATE_SIGN_M_DET_MASK | \ + LIS2DS12_FUNC_CK_GATE_STEP_D_MASK) +#define LIS2DS12_WAKE_UP_IA_MASK 0x40 +#define LIS2DS12_DOUBLE_TAP_MASK 0x10 +#define LIS2DS12_TAP_MASK 0x08 +#define LIS2DS12_6D_IA_MASK 0x04 +#define LIS2DS12_FF_IA_MASK 0x02 +#define LIS2DS12_DRDY_MASK 0x01 +#define LIS2DS12_EVENT_MASK (LIS2DS12_WAKE_UP_IA_MASK | \ + LIS2DS12_DOUBLE_TAP_MASK | \ + LIS2DS12_TAP_MASK | \ + LIS2DS12_6D_IA_MASK | \ + LIS2DS12_FF_IA_MASK) +#define LIS2DS12_FIFO_SRC_ADDR 0x2f +#define LIS2DS12_FIFO_SRC_FTH_MASK 0x80 + +#define LIS2DS12_EN_BIT 0x01 +#define LIS2DS12_DIS_BIT 0x00 +#define LIS2DS12_ACCEL_ODR 1 +#define LIS2DS12_DEFAULT_ACCEL_FS 2 +#define LIS2DS12_FF_ODR 25 +#define LIS2DS12_STEP_D_ODR 25 +#define LIS2DS12_TILT_ODR 25 +#define LIS2DS12_SIGN_M_ODR 25 +#define LIS2DS12_TAP_ODR 400 +#define LIS2DS12_WAKEUP_ODR 25 +#define LIS2DS12_ACTIVITY_ODR 12 +#define LIS2DS12_MAX_FIFO_LENGHT 256 +#define LIS2DS12_MAX_FIFO_THS (LIS2DS12_MAX_FIFO_LENGHT - 1) +#define LIS2DS12_MAX_CHANNEL_SPEC 5 +#define LIS2DS12_EVENT_CHANNEL_SPEC_SIZE 2 +#define LIS2DS12_MIN_DURATION_MS 1638 + +#define LIS2DS12_DEV_NAME "lis2ds12" +#define SET_BIT(a, b) {a |= (1 << b);} +#define RESET_BIT(a, b) {a &= ~(1 << b);} +#define CHECK_BIT(a, b) (a & (1 << b)) + +enum { + LIS2DS12_ACCEL = 0, + LIS2DS12_STEP_C, + LIS2DS12_TAP, + LIS2DS12_DOUBLE_TAP, + LIS2DS12_STEP_D, + LIS2DS12_TILT, + LIS2DS12_SIGN_M, + LIS2DS12_SENSORS_NUMB, +}; + +#define ST_LIS2DS12_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &lis2ds12_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_LIS2DS12_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + lis2ds12_sysfs_get_hwfifo_enabled,\ + lis2ds12_sysfs_set_hwfifo_enabled, 0); + +#define ST_LIS2DS12_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + lis2ds12_sysfs_get_hwfifo_watermark,\ + lis2ds12_sysfs_set_hwfifo_watermark, 0); + +#define ST_LIS2DS12_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + lis2ds12_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_LIS2DS12_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + lis2ds12_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_LIS2DS12_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + lis2ds12_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +#define LIS2DS12_TX_MAX_LENGTH 12 +#define LIS2DS12_RX_MAX_LENGTH 8193 +#define LIS2DS12_EWMA_DIV 128 + +struct lis2ds12_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[LIS2DS12_RX_MAX_LENGTH]; + u8 tx_buf[LIS2DS12_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct lis2ds12_data; + +struct lis2ds12_transfer_function { + int (*write)(struct lis2ds12_data *cdata, u8 reg_addr, int len, + u8 *data, bool b_lock); + int (*read)(struct lis2ds12_data *cdata, u8 reg_addr, int len, + u8 *data, bool b_lock); +}; + +struct lis2ds12_sensor_data { + struct lis2ds12_data *cdata; + const char *name; + s64 timestamp; + u8 enabled; + u32 odr; + u32 gain; + u8 sindex; + u8 sample_to_discard; +}; + +struct lis2ds12_data { + const char *name; + u8 drdy_int_pin; + bool spi_3wire; + u8 selftest_status; + u8 hwfifo_enabled; + u8 hwfifo_watermark; + u8 power_mode; + u8 enabled_sensor; + u32 common_odr; + int irq; + s64 timestamp; + s64 accel_deltatime; + s64 sample_timestamp; + u8 *fifo_data; + u16 fifo_size; + u64 samples; + u8 std_level; + struct mutex fifo_lock; + struct device *dev; + struct iio_dev *iio_sensors_dev[LIS2DS12_SENSORS_NUMB]; + struct iio_trigger *iio_trig[LIS2DS12_SENSORS_NUMB]; + struct mutex regs_lock; + const struct lis2ds12_transfer_function *tf; + struct lis2ds12_transfer_buffer tb; +}; + +static inline s64 lis2ds12_get_time_ns(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +int lis2ds12_common_probe(struct lis2ds12_data *cdata, int irq); +#ifdef CONFIG_PM +int lis2ds12_common_suspend(struct lis2ds12_data *cdata); +int lis2ds12_common_resume(struct lis2ds12_data *cdata); +#endif +int lis2ds12_allocate_rings(struct lis2ds12_data *cdata); +int lis2ds12_allocate_triggers(struct lis2ds12_data *cdata, + const struct iio_trigger_ops *trigger_ops); +int lis2ds12_trig_set_state(struct iio_trigger *trig, bool state); +int lis2ds12_read_register(struct lis2ds12_data *cdata, u8 reg_addr, + int data_len, u8 *data, bool b_lock); +int lis2ds12_update_drdy_irq(struct lis2ds12_sensor_data *sdata, bool state); +int lis2ds12_set_enable(struct lis2ds12_sensor_data *sdata, bool enable); +int lis2ds12_update_fifo(struct lis2ds12_data *cdata, u16 watermark); +void lis2ds12_common_remove(struct lis2ds12_data *cdata, int irq); +void lis2ds12_read_xyz(struct lis2ds12_data *cdata); +void lis2ds12_read_fifo(struct lis2ds12_data *cdata, bool check_fifo_len); +void lis2ds12_read_step_c(struct lis2ds12_data *cdata); +void lis2ds12_deallocate_rings(struct lis2ds12_data *cdata); +void lis2ds12_deallocate_triggers(struct lis2ds12_data *cdata); + +#endif /* __LIS2DS12_H */ diff --git a/drivers/iio/stm/accel/st_lis2ds12_buffer.c b/drivers/iio/stm/accel/st_lis2ds12_buffer.c new file mode 100644 index 000000000000..248c891ca448 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2ds12_buffer.c @@ -0,0 +1,236 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2ds12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2015 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2ds12.h" + +#define LIS2DS12_ACCEL_BUFFER_SIZE \ + ALIGN(LIS2DS12_FIFO_BYTE_FOR_SAMPLE + LIS2DS12_TIMESTAMP_SIZE, \ + LIS2DS12_TIMESTAMP_SIZE) +#define LIS2DS12_STEP_C_BUFFER_SIZE \ + ALIGN(LIS2DS12_FIFO_BYTE_X_AXIS + LIS2DS12_TIMESTAMP_SIZE, \ + LIS2DS12_TIMESTAMP_SIZE) + +static void lis2ds12_push_accel_data(struct lis2ds12_data *cdata, + u8 *acc_buf, u16 read_length) +{ + size_t offset; + uint16_t i, j, k; + u8 buffer[LIS2DS12_ACCEL_BUFFER_SIZE], out_buf_index; + struct iio_dev *indio_dev = cdata->iio_sensors_dev[LIS2DS12_ACCEL]; + u32 delta_ts = div_s64(cdata->accel_deltatime, cdata->hwfifo_watermark); + + for (i = 0; i < read_length; i += LIS2DS12_FIFO_BYTE_FOR_SAMPLE) { + /* Skip first samples. */ + if (unlikely(++cdata->samples <= cdata->std_level)) { + cdata->sample_timestamp += delta_ts; + continue; + } + + for (j = 0, out_buf_index = 0; j < LIS2DS12_FIFO_NUM_AXIS; + j++) { + k = i + LIS2DS12_FIFO_BYTE_X_AXIS * j; + if (test_bit(j, indio_dev->active_scan_mask)) { + memcpy(&buffer[out_buf_index], + &acc_buf[k], + LIS2DS12_FIFO_BYTE_X_AXIS); + out_buf_index += LIS2DS12_FIFO_BYTE_X_AXIS; + } + } + + if (indio_dev->scan_timestamp) { + offset = indio_dev->scan_bytes / sizeof(s64) - 1; + ((s64 *)buffer)[offset] = cdata->sample_timestamp; + cdata->sample_timestamp += delta_ts; + } + + iio_push_to_buffers(indio_dev, buffer); + } +} + +void lis2ds12_read_xyz(struct lis2ds12_data *cdata) +{ + int err; + u8 xyz_buf[LIS2DS12_FIFO_BYTE_FOR_SAMPLE]; + + err = lis2ds12_read_register(cdata, LIS2DS12_OUTX_L_ADDR, + LIS2DS12_FIFO_BYTE_FOR_SAMPLE, xyz_buf, true); + if (err < 0) + return; + + cdata->sample_timestamp = cdata->timestamp; + lis2ds12_push_accel_data(cdata, xyz_buf, LIS2DS12_FIFO_BYTE_FOR_SAMPLE); +} + +void lis2ds12_read_fifo(struct lis2ds12_data *cdata, bool check_fifo_len) +{ + int err; + u8 fifo_src[2]; + u16 read_len; +#if (CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read, extra_bytes; +#endif /* CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO */ + + err = lis2ds12_read_register(cdata, LIS2DS12_FIFO_SRC, 2, + fifo_src, true); + if (err < 0) + return; + + read_len = (fifo_src[0] & LIS2DS12_FIFO_SRC_DIFF_MASK) ? + (1 << 8) : 0; + read_len |= fifo_src[1]; + read_len *= LIS2DS12_FIFO_BYTE_FOR_SAMPLE; + + if (read_len == 0) + return; + +#if (CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO == 0) + err = lis2ds12_read_register(cdata, LIS2DS12_OUTX_L_ADDR, read_len, + cdata->fifo_data, true); + if (err < 0) + return; +#else /* CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + extra_bytes = (data_to_read % LIS2DS12_FIFO_BYTE_FOR_SAMPLE); + if (extra_bytes != 0) { + data_to_read -= extra_bytes; + + if (data_to_read < LIS2DS12_FIFO_BYTE_FOR_SAMPLE) + data_to_read = LIS2DS12_FIFO_BYTE_FOR_SAMPLE; + } + + err = lis2ds12_read_register(cdata, LIS2DS12_OUTX_L_ADDR, data_to_read, + &cdata->fifo_data[read_len - data_remaining], true); + if (err < 0) + return; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO */ + + lis2ds12_push_accel_data(cdata, cdata->fifo_data, read_len); +} + +void lis2ds12_read_step_c(struct lis2ds12_data *cdata) +{ + int err; + int64_t timestamp = 0; + char buffer[LIS2DS12_STEP_C_BUFFER_SIZE]; + struct iio_dev *indio_dev = cdata->iio_sensors_dev[LIS2DS12_STEP_C]; + + err = lis2ds12_read_register(cdata, (u8)indio_dev->channels[0].address, + 2, buffer, true); + if (err < 0) + goto lis2ds12_step_counter_done; + + timestamp = cdata->timestamp; + if (indio_dev->scan_timestamp) + *(s64 *) ((u8 *) buffer + + ALIGN(LIS2DS12_FIFO_BYTE_X_AXIS, sizeof(s64))) = + timestamp; + + iio_push_to_buffers(indio_dev, buffer); + +lis2ds12_step_counter_done: + iio_trigger_notify_done(indio_dev->trig); +} + +static inline irqreturn_t lis2ds12_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int lis2ds12_trig_set_state(struct iio_trigger *trig, bool state) +{ + int err; + struct lis2ds12_sensor_data *sdata; + + sdata = iio_priv(iio_trigger_get_drvdata(trig)); + err = lis2ds12_update_drdy_irq(sdata, state); + + return (err < 0) ? err : 0; +} + +static int lis2ds12_buffer_preenable(struct iio_dev *indio_dev) +{ + int err; + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + err = lis2ds12_set_enable(sdata, true); + if (err < 0) + return err; + + return 0; +} + +static int lis2ds12_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + err = lis2ds12_set_enable(sdata, false); + if (err < 0) + return err; + + return 0; +} + +static const struct iio_buffer_setup_ops lis2ds12_buffer_setup_ops = { + .preenable = &lis2ds12_buffer_preenable, + .postdisable = &lis2ds12_buffer_postdisable, +}; + +int lis2ds12_allocate_rings(struct lis2ds12_data *cdata) +{ + int err, i; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) { + err = iio_triggered_buffer_setup( + cdata->iio_sensors_dev[i], + &lis2ds12_handler_empty, + NULL, + &lis2ds12_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup; + } + + return 0; + +buffer_cleanup: + for (i--; i >= 0; i--) + iio_triggered_buffer_cleanup(cdata->iio_sensors_dev[i]); + + return err; +} + +void lis2ds12_deallocate_rings(struct lis2ds12_data *cdata) +{ + int i; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) + iio_triggered_buffer_cleanup(cdata->iio_sensors_dev[i]); +} diff --git a/drivers/iio/stm/accel/st_lis2ds12_core.c b/drivers/iio/stm/accel/st_lis2ds12_core.c new file mode 100644 index 000000000000..8e666e5b2b17 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2ds12_core.c @@ -0,0 +1,1528 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2ds12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2015 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2ds12.h" + +#define LIS2DS12_FS_LIST_NUM 4 +enum { + LIS2DS12_LP_MODE = 0, + LIS2DS12_HR_MODE, + LIS2DS12_MODE_COUNT, +}; + +#define LIS2DS12_ADD_CHANNEL(device_type, modif, index, mod, endian, sbits,\ + rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +struct lis2ds12_odr_reg { + u32 hz; + u8 value; + /* Skip samples. */ + u8 std_level; +}; + +static const struct lis2ds12_odr_table_t { + u8 addr; + u8 mask; + struct lis2ds12_odr_reg odr_avl[LIS2DS12_MODE_COUNT][LIS2DS12_ODR_LP_LIST_NUM]; +} lis2ds12_odr_table = { + .addr = LIS2DS12_ODR_ADDR, + .mask = LIS2DS12_ODR_MASK, + + /* + * ODR values for Low Power Mode + */ + .odr_avl[LIS2DS12_LP_MODE][0] = {.hz = 0, + .value = LIS2DS12_ODR_POWER_OFF_VAL, + .std_level = 0,}, + .odr_avl[LIS2DS12_LP_MODE][1] = {.hz = 1, + .value = LIS2DS12_ODR_1HZ_LP_VAL, + .std_level = 0,}, + .odr_avl[LIS2DS12_LP_MODE][2] = {.hz = 12, + .value = LIS2DS12_ODR_12HZ_LP_VAL, + .std_level = 4,}, + .odr_avl[LIS2DS12_LP_MODE][3] = {.hz = 25, + .value = LIS2DS12_ODR_25HZ_LP_VAL, + .std_level = 8,}, + .odr_avl[LIS2DS12_LP_MODE][4] = {.hz = 50, + .value = LIS2DS12_ODR_50HZ_LP_VAL, + .std_level = 24,}, + .odr_avl[LIS2DS12_LP_MODE][5] = {.hz = 100, + .value = LIS2DS12_ODR_100HZ_LP_VAL, + .std_level = 24,}, + .odr_avl[LIS2DS12_LP_MODE][6] = {.hz = 200, + .value = LIS2DS12_ODR_200HZ_LP_VAL, + .std_level = 32,}, + .odr_avl[LIS2DS12_LP_MODE][7] = {.hz = 400, + .value = LIS2DS12_ODR_400HZ_LP_VAL, + .std_level = 48,}, + .odr_avl[LIS2DS12_LP_MODE][8] = {.hz = 800, + .value = LIS2DS12_ODR_800HZ_LP_VAL, + .std_level = 50,}, + + /* + * ODR values for High Resolution Mode + */ + .odr_avl[LIS2DS12_HR_MODE][0] = {.hz = 0, + .value = LIS2DS12_ODR_POWER_OFF_VAL, + .std_level = 0,}, + .odr_avl[LIS2DS12_HR_MODE][1] = {.hz = 12, + .value = LIS2DS12_ODR_12_5HZ_HR_VAL, + .std_level = 4,}, + .odr_avl[LIS2DS12_HR_MODE][2] = {.hz = 25, + .value = LIS2DS12_ODR_25HZ_HR_VAL, + .std_level = 8,}, + .odr_avl[LIS2DS12_HR_MODE][3] = {.hz = 50, + .value = LIS2DS12_ODR_50HZ_HR_VAL, + .std_level = 24,}, + .odr_avl[LIS2DS12_HR_MODE][4] = {.hz = 100, + .value = LIS2DS12_ODR_100HZ_HR_VAL, + .std_level = 24,}, + .odr_avl[LIS2DS12_HR_MODE][5] = {.hz = 200, + .value = LIS2DS12_ODR_200HZ_HR_VAL, + .std_level = 32,}, + .odr_avl[LIS2DS12_HR_MODE][6] = {.hz = 400, + .value = LIS2DS12_ODR_400HZ_HR_VAL, + .std_level = 48,}, + .odr_avl[LIS2DS12_HR_MODE][7] = {.hz = 800, + .value = LIS2DS12_ODR_800HZ_HR_VAL, + .std_level = 50,}, +}; + +struct lis2ds12_fs_reg { + unsigned int gain; + u8 value; + int urv; +}; + +static struct lis2ds12_fs_table { + u8 addr; + u8 mask; + struct lis2ds12_fs_reg fs_avl[LIS2DS12_FS_LIST_NUM]; +} lis2ds12_fs_table = { + .addr = LIS2DS12_FS_ADDR, + .mask = LIS2DS12_FS_MASK, + .fs_avl[0] = { + .gain = LIS2DS12_FS_2G_GAIN, + .value = LIS2DS12_FS_2G_VAL, + .urv = 2, + }, + .fs_avl[1] = { + .gain = LIS2DS12_FS_4G_GAIN, + .value = LIS2DS12_FS_4G_VAL, + .urv = 4, + }, + .fs_avl[2] = { + .gain = LIS2DS12_FS_8G_GAIN, + .value = LIS2DS12_FS_8G_VAL, + .urv = 8, + }, + .fs_avl[3] = { + .gain = LIS2DS12_FS_16G_GAIN, + .value = LIS2DS12_FS_16G_VAL, + .urv = 16, + }, +}; + +static const struct iio_event_spec singol_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, +}; + +const struct iio_event_spec lis2ds12_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct lis2ds12_sensors_table { + const char *name; + const char *description; + const u32 min_odr_hz; + const u8 iio_channel_size; + const struct iio_chan_spec iio_channel[LIS2DS12_MAX_CHANNEL_SPEC]; +} lis2ds12_sensors_table[LIS2DS12_SENSORS_NUMB] = { + [LIS2DS12_ACCEL] = { + .name = "accel", + .description = "ST LIS2DS12 Accelerometer Sensor", + .min_odr_hz = LIS2DS12_ACCEL_ODR, + .iio_channel = { + LIS2DS12_ADD_CHANNEL(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, LIS2DS12_OUTX_L_ADDR, 's'), + LIS2DS12_ADD_CHANNEL(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, LIS2DS12_OUTY_L_ADDR, 's'), + LIS2DS12_ADD_CHANNEL(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, LIS2DS12_OUTZ_L_ADDR, 's'), + ST_LIS2DS12_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) + }, + .iio_channel_size = LIS2DS12_MAX_CHANNEL_SPEC, + }, + [LIS2DS12_STEP_C] = { + .name = "step_c", + .description = "ST LIS2DS12 Step Counter Sensor", + .min_odr_hz = LIS2DS12_STEP_D_ODR, + .iio_channel = { + { + .type = IIO_STEP_COUNTER, + .channel = 0, + .modified = 0, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .address = LIS2DS12_STEP_C_OUT_L_ADDR, + .scan_index = 0, + .channel2 = IIO_NO_MOD, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = LIS2DS12_EVENT_CHANNEL_SPEC_SIZE, + }, + [LIS2DS12_TAP] = { + .name = "tap", + .description = "ST LIS2DS12 Tap Sensor", + .min_odr_hz = LIS2DS12_TAP_ODR, + .iio_channel = { + { + .type = IIO_TAP, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = LIS2DS12_EVENT_CHANNEL_SPEC_SIZE, + }, + [LIS2DS12_DOUBLE_TAP] = { + .name = "tap_tap", + .description = "ST LIS2DS12 Double Tap Sensor", + .min_odr_hz = LIS2DS12_TAP_ODR, + .iio_channel = { + { + .type = IIO_TAP_TAP, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = LIS2DS12_EVENT_CHANNEL_SPEC_SIZE, + }, + [LIS2DS12_STEP_D] = { + .name = "step_d", + .description = "ST LIS2DS12 Step Detector Sensor", + .min_odr_hz = LIS2DS12_STEP_D_ODR, + .iio_channel = { + { + .type = IIO_STEP_DETECTOR, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = LIS2DS12_EVENT_CHANNEL_SPEC_SIZE, + }, + [LIS2DS12_TILT] = { + .name = "tilt", + .description = "ST LIS2DS12 Tilt Sensor", + .min_odr_hz = LIS2DS12_TILT_ODR, + .iio_channel = { + { + .type = IIO_TILT, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), + }, + .iio_channel_size = LIS2DS12_EVENT_CHANNEL_SPEC_SIZE, + }, + [LIS2DS12_SIGN_M] = { + .name = "sign_motion", + .description = "ST LIS2DS12 Significant Motion Sensor", + .min_odr_hz = LIS2DS12_SIGN_M_ODR, + .iio_channel = { + { + .type = IIO_SIGN_MOTION, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) + }, + .iio_channel_size = LIS2DS12_EVENT_CHANNEL_SPEC_SIZE, + }, +}; + +static const struct { + char *mode_str; + u8 streg_val; +} lis2ds12_selftest_table[] = { + { + .mode_str = "normal-mode", + .streg_val = LIS2DS12_SELFTEST_NORMAL, + }, + { + .mode_str = "positive-sign", + .streg_val = LIS2DS12_SELFTEST_POS_SIGN, + }, + { + .mode_str = "negative-sign", + .streg_val = LIS2DS12_SELFTEST_NEG_SIGN, + }, +}; + +int lis2ds12_read_register(struct lis2ds12_data *cdata, u8 reg_addr, + int data_len, u8 *data, bool b_lock) +{ + return cdata->tf->read(cdata, reg_addr, data_len, data, b_lock); +} + +static int lis2ds12_write_register(struct lis2ds12_data *cdata, u8 reg_addr, + u8 mask, u8 data, bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = lis2ds12_read_register(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} + +static int lis2ds12_write_advanced_cfg_regs(struct lis2ds12_data *cdata, + u8 reg_addr, u8 *data, int len) +{ + int err = 0, err2 = 0; + int count = 0; + + mutex_lock(&cdata->regs_lock); + + err = lis2ds12_write_register(cdata, LIS2DS12_FUNC_CFG_ENTER_ADDR, + LIS2DS12_FUNC_CFG_EN_MASK, LIS2DS12_EN_BIT, false); + if (err < 0) + goto lis2ds12_write_advanced_cfg_regs_mutex_unlock; + + err = cdata->tf->write(cdata, reg_addr, len, data, false); + if (err < 0) + goto lis2ds12_write_advanced_cfg_regs_switch_bank_regs; + + err = lis2ds12_write_register(cdata, LIS2DS12_FUNC_CFG_EXIT_ADDR, + LIS2DS12_FUNC_CFG_EN_MASK, LIS2DS12_DIS_BIT, false); + if (err < 0) + goto lis2ds12_write_advanced_cfg_regs_switch_bank_regs; + + mutex_unlock(&cdata->regs_lock); + + return 0; + +lis2ds12_write_advanced_cfg_regs_switch_bank_regs: + do { + msleep(200); + err2 = lis2ds12_write_register(cdata, LIS2DS12_FUNC_CFG_EXIT_ADDR, + LIS2DS12_FUNC_CFG_EN_MASK, LIS2DS12_DIS_BIT, false); + } while (err2 < 0 && count++ < 10); + +lis2ds12_write_advanced_cfg_regs_mutex_unlock: + mutex_unlock(&cdata->regs_lock); + + return err; +} + +int lis2ds12_set_axis_enable(struct lis2ds12_sensor_data *sdata, u8 value) +{ + return 0; +} +EXPORT_SYMBOL(lis2ds12_set_axis_enable); + +int lis2ds12_set_fifo_mode(struct lis2ds12_data *cdata, enum fifo_mode fm) +{ + u8 reg_value; + + switch (fm) { + case BYPASS: + reg_value = LIS2DS12_FIFO_MODE_BYPASS; + break; + case CONTINUOS: + reg_value = LIS2DS12_FIFO_MODE_CONTINUOS; + break; + default: + return -EINVAL; + } + + return lis2ds12_write_register(cdata, LIS2DS12_FIFO_MODE_ADDR, + LIS2DS12_FIFO_MODE_MASK, reg_value, true); +} +EXPORT_SYMBOL(lis2ds12_set_fifo_mode); + +int lis2ds12_update_event_functions(struct lis2ds12_data *cdata) +{ + u8 reg_val = 0; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_SIGN_M)) + reg_val |= LIS2DS12_FUNC_CTRL_SIGN_MOT_MASK; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_TILT)) + reg_val |= LIS2DS12_FUNC_CTRL_TILT_MASK; + + if ((CHECK_BIT(cdata->enabled_sensor, LIS2DS12_STEP_D)) || + (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_STEP_C))) + reg_val |= LIS2DS12_FUNC_CTRL_STEP_CNT_MASK; + + return lis2ds12_write_register(cdata, + LIS2DS12_FUNC_CTRL_ADDR, + LIS2DS12_FUNC_CTRL_EV_MASK, + reg_val >> __ffs(LIS2DS12_FUNC_CTRL_EV_MASK), true); +} + +int lis2ds12_set_fs(struct lis2ds12_sensor_data *sdata, unsigned int fs) +{ + int err, i; + + for (i = 0; i < LIS2DS12_FS_LIST_NUM; i++) { + if (lis2ds12_fs_table.fs_avl[i].urv == fs) + break; + } + + if (i == LIS2DS12_FS_LIST_NUM) + return -EINVAL; + + err = lis2ds12_write_register(sdata->cdata, + lis2ds12_fs_table.addr, + lis2ds12_fs_table.mask, + lis2ds12_fs_table.fs_avl[i].value, true); + if (err < 0) + return err; + + sdata->gain = lis2ds12_fs_table.fs_avl[i].gain; + + return 0; +} + +static int lis2ds12_set_selftest_mode(struct lis2ds12_sensor_data *sdata, + u8 index) +{ + return lis2ds12_write_register(sdata->cdata, LIS2DS12_SELFTEST_ADDR, + LIS2DS12_SELFTEST_MASK, + lis2ds12_selftest_table[index].streg_val, true); +} + +u8 lis2ds12_event_irq1_value(struct lis2ds12_data *cdata) +{ + u8 value = 0x0; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_DOUBLE_TAP)) + value |= LIS2DS12_INT1_TAP_MASK; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_TAP)) + value |= LIS2DS12_INT1_S_TAP_MASK | LIS2DS12_INT1_TAP_MASK; + + return value; +} + +u8 lis2ds12_event_irq2_value(struct lis2ds12_data *cdata) +{ + u8 value = 0x0; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_TILT)) + value |= LIS2DS12_INT2_TILT_MASK; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_SIGN_M)) + value |= LIS2DS12_INT2_SIG_MOT_DET_MASK; + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_STEP_D) || + CHECK_BIT(cdata->enabled_sensor, LIS2DS12_STEP_C)) + value |= LIS2DS12_INT2_STEP_DET_MASK; + + return value; +} + +int lis2ds12_write_max_odr(struct lis2ds12_sensor_data *sdata) { + int err, i; + u32 max_odr = 0; + u8 power_mode = sdata->cdata->power_mode; + struct lis2ds12_sensor_data *t_sdata; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) + if (CHECK_BIT(sdata->cdata->enabled_sensor, i)) { + t_sdata = iio_priv(sdata->cdata->iio_sensors_dev[i]); + if (t_sdata->odr > max_odr) + max_odr = t_sdata->odr; + } + + for (i = 0; i < LIS2DS12_ODR_LP_LIST_NUM; i++) { + if (lis2ds12_odr_table.odr_avl[power_mode][i].hz >= max_odr) + break; + } + + if (i == LIS2DS12_ODR_LP_LIST_NUM) + return -EINVAL; + + err = lis2ds12_write_register(sdata->cdata, + lis2ds12_odr_table.addr, + lis2ds12_odr_table.mask, + lis2ds12_odr_table.odr_avl[power_mode][i].value, true); + if (err < 0) + return err; + + sdata->cdata->common_odr = max_odr; + sdata->cdata->std_level = lis2ds12_odr_table.odr_avl[power_mode][i].std_level; + + switch (max_odr) { + case 0: + sdata->cdata->accel_deltatime = 0; + break; + + case 12: + sdata->cdata->accel_deltatime = + 80000000LL * sdata->cdata->hwfifo_watermark; + break; + + default: + sdata->cdata->accel_deltatime = + div_s64(1000000000LL, max_odr) * + sdata->cdata->hwfifo_watermark; + break; + } + + return 0; +} + +int lis2ds12_update_drdy_irq(struct lis2ds12_sensor_data *sdata, bool state) +{ + u8 reg_addr, reg_val, reg_mask; + + switch (sdata->sindex) { + case LIS2DS12_TAP: + case LIS2DS12_DOUBLE_TAP: + reg_val = lis2ds12_event_irq1_value(sdata->cdata); + reg_addr = LIS2DS12_CTRL4_INT1_PAD_ADDR; + reg_mask = LIS2DS12_INT1_EVENTS_MASK; + + break; + + case LIS2DS12_SIGN_M: + case LIS2DS12_TILT: + case LIS2DS12_STEP_D: + case LIS2DS12_STEP_C: + reg_val = lis2ds12_event_irq2_value(sdata->cdata); + reg_addr = LIS2DS12_CTRL5_INT2_PAD_ADDR; + reg_mask = LIS2DS12_INT2_EVENTS_MASK; + + break; + + case LIS2DS12_ACCEL: + reg_addr = LIS2DS12_CTRL4_INT1_PAD_ADDR; + reg_mask = (sdata->cdata->hwfifo_enabled) ? + LIS2DS12_INT1_FTH_MASK: + LIS2DS12_DRDY_MASK; + if (state) + reg_val = LIS2DS12_EN_BIT; + else + reg_val = LIS2DS12_DIS_BIT; + + break; + + default: + return -EINVAL; + } + + return lis2ds12_write_register(sdata->cdata, reg_addr, reg_mask, + reg_val, true); +} +EXPORT_SYMBOL(lis2ds12_update_drdy_irq); + +int lis2ds12_update_fifo(struct lis2ds12_data *cdata, u16 watermark) +{ + int err; + int fifo_size; + struct iio_dev *indio_dev; + + indio_dev = cdata->iio_sensors_dev[LIS2DS12_ACCEL]; + cdata->timestamp = lis2ds12_get_time_ns(indio_dev); + cdata->sample_timestamp = cdata->timestamp; + cdata->samples = 0; + + err = lis2ds12_write_register(cdata, LIS2DS12_FIFO_THS_ADDR, + LIS2DS12_FIFO_THS_MASK, + watermark, true); + if (err < 0) + return err; + + if (cdata->fifo_data) + kfree(cdata->fifo_data); + + cdata->fifo_data = 0; + + fifo_size = watermark * LIS2DS12_FIFO_BYTE_FOR_SAMPLE; + if (fifo_size > 0) { + cdata->fifo_data = kmalloc(fifo_size, GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + + cdata->fifo_size = fifo_size; + } + + return lis2ds12_set_fifo_mode(cdata, CONTINUOS); +} +EXPORT_SYMBOL(lis2ds12_update_fifo); + +int lis2ds12_set_enable(struct lis2ds12_sensor_data *sdata, bool state) +{ + int err = 0; + u8 mode; + + if (sdata->enabled == state) + return 0; + + /* + * Start assuming the sensor enabled if state == true. + * It will be restored if an error occur. + */ + if (state) { + SET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + mode = CONTINUOS; + } else { + RESET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + mode = BYPASS; + } + + switch (sdata->sindex) { + case LIS2DS12_TAP: + if (state && CHECK_BIT(sdata->cdata->enabled_sensor, + LIS2DS12_DOUBLE_TAP)) { + err = -EINVAL; + + goto enable_sensor_error; + } + + break; + + case LIS2DS12_DOUBLE_TAP: + if (state && CHECK_BIT(sdata->cdata->enabled_sensor, + LIS2DS12_TAP)) { + err = -EINVAL; + + goto enable_sensor_error; + } + + break; + + case LIS2DS12_TILT: + case LIS2DS12_SIGN_M: + case LIS2DS12_STEP_D: + case LIS2DS12_STEP_C: + err = lis2ds12_update_event_functions(sdata->cdata); + if (err < 0) + goto enable_sensor_error; + + break; + + case LIS2DS12_ACCEL: + break; + + default: + return -EINVAL; + } + + err = lis2ds12_update_drdy_irq(sdata, state); + if (err < 0) + goto enable_sensor_error; + + err = lis2ds12_set_fifo_mode(sdata->cdata, mode); + if (err < 0) + return err; + + err = lis2ds12_write_max_odr(sdata); + if (err < 0) + goto enable_sensor_error; + + sdata->enabled = state; + + return 0; + +enable_sensor_error: + if (state) { + RESET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + } else { + SET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + } + + return err; +} +EXPORT_SYMBOL(lis2ds12_set_enable); + +int lis2ds12_init_sensors(struct lis2ds12_data *cdata) +{ + int err, i; + struct lis2ds12_sensor_data *sdata; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) { + sdata = iio_priv(cdata->iio_sensors_dev[i]); + + err = lis2ds12_set_enable(sdata, false); + if (err < 0) + return err; + + if (sdata->sindex == LIS2DS12_ACCEL) { + err = lis2ds12_set_fs(sdata, LIS2DS12_DEFAULT_ACCEL_FS); + if (err < 0) + return err; + } + } + + cdata->selftest_status = 0; + + /* + * Soft reset the device on power on. + */ + err = lis2ds12_write_register(cdata, LIS2DS12_SOFT_RESET_ADDR, + LIS2DS12_SOFT_RESET_MASK, + LIS2DS12_EN_BIT, true); + if (err < 0) + return err; + + if (cdata->spi_3wire) { + u8 data = LIS2DS12_ADD_INC_MASK | LIS2DS12_SIM_MASK; + + err = cdata->tf->write(cdata, LIS2DS12_SIM_ADDR, 1, &data, + false); + if (err < 0) + return err; + } + + /* + * Enable latched interrupt mode. + */ + err = lis2ds12_write_register(cdata, LIS2DS12_LIR_ADDR, + LIS2DS12_LIR_MASK, + LIS2DS12_EN_BIT, true); + if (err < 0) + return err; + + /* + * Enable block data update feature. + */ + err = lis2ds12_write_register(cdata, LIS2DS12_BDU_ADDR, + LIS2DS12_BDU_MASK, + LIS2DS12_EN_BIT, true); + if (err < 0) + return err; + + /* + * Route interrupt from INT2 to INT1 pin. + */ + err = lis2ds12_write_register(cdata, LIS2DS12_INT2_ON_INT1_ADDR, + LIS2DS12_INT2_ON_INT1_MASK, + LIS2DS12_EN_BIT, true); + if (err < 0) + return err; + + /* + * Configure default free fall event threshold. + */ + err = lis2ds12_write_register(sdata->cdata, LIS2DS12_FREE_FALL_ADDR, + LIS2DS12_FREE_FALL_THS_MASK, + LIS2DS12_FREE_FALL_THS_DEFAULT, true); + if (err < 0) + return err; + + /* + * Configure default free fall event duration. + */ + err = lis2ds12_write_register(sdata->cdata, LIS2DS12_FREE_FALL_ADDR, + LIS2DS12_FREE_FALL_DUR_MASK, + LIS2DS12_FREE_FALL_DUR_DEFAULT, true); + if (err < 0) + return err; + + /* + * Configure Tap event recognition on all direction (X, Y and Z axes). + */ + err = lis2ds12_write_register(sdata->cdata, LIS2DS12_TAP_AXIS_ADDR, + LIS2DS12_TAP_AXIS_MASK, + LIS2DS12_TAP_AXIS_ANABLE_ALL, true); + if (err < 0) + return err; + + /* + * Configure default threshold for Tap event recognition. + */ + err = lis2ds12_write_register(sdata->cdata, LIS2DS12_TAP_THS_ADDR, + LIS2DS12_TAP_THS_MASK, + LIS2DS12_TAP_THS_DEFAULT, true); + if (err < 0) + return err; + + /* + * Configure default threshold for Wake Up event recognition. + */ + err = lis2ds12_write_register(sdata->cdata, LIS2DS12_WAKE_UP_THS_ADDR, + LIS2DS12_WAKE_UP_THS_WU_MASK, + LIS2DS12_WAKE_UP_THS_WU_DEFAULT, true); + if (err < 0) + return err; + + return 0; +} + +static ssize_t lis2ds12_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct lis2ds12_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->odr); +} + +ssize_t lis2ds12_set_sampling_frequency(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + int err; + u8 power_mode; + unsigned int odr, i; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + if (sdata->odr == odr) + return count; + + power_mode = sdata->cdata->power_mode; + for (i = 0; i < LIS2DS12_ODR_LP_LIST_NUM; i++) { + if (lis2ds12_odr_table.odr_avl[power_mode][i].hz >= odr) + break; + } + if (i == LIS2DS12_ODR_LP_LIST_NUM) + return -EINVAL; + + mutex_lock(&indio_dev->mlock); + sdata->odr = lis2ds12_odr_table.odr_avl[power_mode][i].hz; + mutex_unlock(&indio_dev->mlock); + + err = lis2ds12_write_max_odr(sdata); + + return (err < 0) ? err : count; +} + +static ssize_t lis2ds12_get_sampling_frequency_avail(struct device *dev, + struct device_attribute + *attr, char *buf) +{ + int i, len = 0, mode_count, mode; + struct lis2ds12_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + mode = sdata->cdata->power_mode; + mode_count = (mode == LIS2DS12_LP_MODE) ? + LIS2DS12_ODR_LP_LIST_NUM : LIS2DS12_ODR_HR_LIST_NUM; + + for (i = 1; i < mode_count; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + lis2ds12_odr_table.odr_avl[mode][i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t lis2ds12_get_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < LIS2DS12_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + lis2ds12_fs_table.fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static int lis2ds12_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[2]; + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = lis2ds12_set_enable(sdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + msleep(40); + + err = lis2ds12_read_register(sdata->cdata, ch->address, 2, + outdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + err = lis2ds12_set_enable(sdata, false); + mutex_unlock(&indio_dev->mlock); + + if (err < 0) + return err; + + return IIO_VAL_INT; + + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->gain; + + return IIO_VAL_INT_PLUS_MICRO; + + default: + return -EINVAL; + } + + return 0; +} + +static int lis2ds12_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long mask) +{ + int err, i; + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + for (i = 0; i < LIS2DS12_FS_LIST_NUM; i++) { + if (lis2ds12_fs_table.fs_avl[i].gain == val2) + break; + } + + err = lis2ds12_set_fs(sdata, lis2ds12_fs_table.fs_avl[i].urv); + mutex_unlock(&indio_dev->mlock); + + break; + + default: + return -EINVAL; + } + + return err; +} + +static ssize_t lis2ds12_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->hwfifo_enabled); +} + +ssize_t lis2ds12_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + int err = 0, enable = 0; + u8 mode = BYPASS; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &enable); + if (err < 0) + return err; + + if (enable != 0x0 && enable != 0x1) + return -EINVAL; + + mode = (enable == 0x0) ? BYPASS : CONTINUOS; + + err = lis2ds12_set_fifo_mode(sdata->cdata, mode); + if (err < 0) + return err; + + sdata->cdata->hwfifo_enabled = enable; + + return count; +} + +static ssize_t lis2ds12_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->hwfifo_watermark); +} + +ssize_t lis2ds12_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, const char *buf, size_t count) +{ + int err = 0, watermark = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > LIS2DS12_MAX_FIFO_THS)) + return -EINVAL; + + mutex_lock(&sdata->cdata->fifo_lock); + err = lis2ds12_update_fifo(sdata->cdata, watermark); + mutex_unlock(&sdata->cdata->fifo_lock); + if (err < 0) + return err; + + sdata->cdata->hwfifo_watermark = watermark; + + return count; +} + +static ssize_t lis2ds12_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +static ssize_t lis2ds12_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", LIS2DS12_MAX_FIFO_THS); +} + +ssize_t lis2ds12_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u64 event_type; + int64_t sensor_last_timestamp; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = lis2ds12_get_time_ns(indio_dev); + + mutex_lock(&sdata->cdata->fifo_lock); + lis2ds12_read_fifo(sdata->cdata, true); + mutex_unlock(&sdata->cdata->fifo_lock); + + sdata->cdata->timestamp = sensor_last_timestamp; + + if (sensor_last_timestamp == sdata->cdata->sample_timestamp) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(IIO_ACCEL, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + sensor_last_timestamp); + + enable_irq(sdata->cdata->irq); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +ssize_t lis2ds12_reset_step_counter(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + return lis2ds12_write_register(sdata->cdata, + LIS2DS12_STEP_C_MINTHS_ADDR, + LIS2DS12_STEP_C_MINTHS_RST_NSTEP_MASK, + LIS2DS12_EN_BIT, true); +} + +static ssize_t lis2ds12_sysfs_set_max_delivery_rate(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u8 duration; + int err; + unsigned int max_delivery_rate; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtouint(buf, 10, &max_delivery_rate); + if (err < 0) + return -EINVAL; + + if (max_delivery_rate == sdata->odr) + return size; + + duration = max_delivery_rate / LIS2DS12_MIN_DURATION_MS; + + err = lis2ds12_write_advanced_cfg_regs(sdata->cdata, + LIS2DS12_STEP_COUNT_DELTA, &duration, 1); + if (err < 0) + return err; + + sdata->odr = max_delivery_rate; + + return size; +} + +static ssize_t lis2ds12_sysfs_get_max_delivery_rate(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->odr); +} + +static ssize_t lis2ds12_get_selftest_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s %s %s\n", lis2ds12_selftest_table[0].mode_str, + lis2ds12_selftest_table[1].mode_str, + lis2ds12_selftest_table[2].mode_str); +} + +static ssize_t lis2ds12_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + u8 status; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + status = sdata->cdata->selftest_status; + return sprintf(buf, "%s\n", lis2ds12_selftest_table[status].mode_str); +} + +static ssize_t lis2ds12_set_selftest_status(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + int err, i; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2ds12_sensor_data *sdata = iio_priv(indio_dev); + + for (i = 0; i < ARRAY_SIZE(lis2ds12_selftest_table); i++) { + if (strncmp(buf, lis2ds12_selftest_table[i].mode_str, + size - 2) == 0) + break; + } + if (i == ARRAY_SIZE(lis2ds12_selftest_table)) + return -EINVAL; + + err = lis2ds12_set_selftest_mode(sdata, i); + if (err < 0) + return err; + + sdata->cdata->selftest_status = i; + + return size; +} + +static ST_LIS2DS12_HWFIFO_ENABLED(); +static ST_LIS2DS12_HWFIFO_WATERMARK(); +static ST_LIS2DS12_HWFIFO_WATERMARK_MIN(); +static ST_LIS2DS12_HWFIFO_WATERMARK_MAX(); +static ST_LIS2DS12_HWFIFO_FLUSH(); + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + lis2ds12_get_sampling_frequency, + lis2ds12_set_sampling_frequency); +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(lis2ds12_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO, + lis2ds12_get_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(reset_counter, S_IWUSR, + NULL, lis2ds12_reset_step_counter, 0); +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + lis2ds12_get_selftest_avail, NULL, 0); +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + lis2ds12_get_selftest_status, + lis2ds12_set_selftest_status, 0); +static IIO_DEVICE_ATTR(max_delivery_rate, S_IWUSR | S_IRUGO, + lis2ds12_sysfs_get_max_delivery_rate, + lis2ds12_sysfs_set_max_delivery_rate, 0); + +static struct attribute *lis2ds12_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + + NULL, +}; + +static struct attribute *lis2ds12_step_c_attributes[] = { + &iio_dev_attr_reset_counter.dev_attr.attr, + &iio_dev_attr_max_delivery_rate.dev_attr.attr, + NULL, +}; +static struct attribute *lis2ds12_step_tap_attributes[] = { + NULL, +}; +static struct attribute *lis2ds12_step_double_tap_attributes[] = { + NULL, +}; +static struct attribute *lis2ds12_step_d_attributes[] = { + NULL, +}; +static struct attribute *lis2ds12_tilt_attributes[] = { + NULL, +}; +static struct attribute *lis2ds12_sign_m_attributes[] = { + NULL, +}; + +static const struct attribute_group lis2ds12_accel_attribute_group = { + .attrs = lis2ds12_accel_attributes, +}; +static const struct attribute_group lis2ds12_step_c_attribute_group = { + .attrs = lis2ds12_step_c_attributes, +}; +static const struct attribute_group lis2ds12_tap_attribute_group = { + .attrs = lis2ds12_step_tap_attributes, +}; +static const struct attribute_group lis2ds12_double_tap_attribute_group = { + .attrs = lis2ds12_step_double_tap_attributes, +}; +static const struct attribute_group lis2ds12_step_d_attribute_group = { + .attrs = lis2ds12_step_d_attributes, +}; +static const struct attribute_group lis2ds12_tilt_attribute_group = { + .attrs = lis2ds12_tilt_attributes, +}; +static const struct attribute_group lis2ds12_sign_m_attribute_group = { + .attrs = lis2ds12_sign_m_attributes, +}; + + +static const struct iio_info lis2ds12_info[LIS2DS12_SENSORS_NUMB] = { + [LIS2DS12_ACCEL] = { + .attrs = &lis2ds12_accel_attribute_group, + .read_raw = &lis2ds12_read_raw, + .write_raw = &lis2ds12_write_raw, + }, + [LIS2DS12_STEP_C] = { + .attrs = &lis2ds12_step_c_attribute_group, + .read_raw = &lis2ds12_read_raw, + }, + [LIS2DS12_TAP] = { + .attrs = &lis2ds12_tap_attribute_group, + }, + [LIS2DS12_DOUBLE_TAP] = { + .attrs = &lis2ds12_double_tap_attribute_group, + }, + [LIS2DS12_STEP_D] = { + .attrs = &lis2ds12_step_d_attribute_group, + }, + [LIS2DS12_TILT] = { + .attrs = &lis2ds12_tilt_attribute_group, + }, + [LIS2DS12_SIGN_M] = { + .attrs = &lis2ds12_sign_m_attribute_group, + }, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops lis2ds12_trigger_ops = { + .set_trigger_state = (&lis2ds12_trig_set_state), +}; +#define LIS2DS12_TRIGGER_OPS (&lis2ds12_trigger_ops) +#else +#define LIS2DS12_TRIGGER_OPS NULL +#endif + +#ifdef CONFIG_OF +static u32 lis2ds12_parse_dt(struct lis2ds12_data *cdata) +{ + u32 val; + struct device_node *np; + + np = cdata->dev->of_node; + if (!np) + return -EINVAL; + + if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && + (val <= 2) && (val > 0)) + cdata->drdy_int_pin = (u8) val; + else + cdata->drdy_int_pin = 1; + + return 0; +} +#endif + +static int lis2ds12_init_interface(struct lis2ds12_data *cdata) +{ + struct device_node *np = cdata->dev->of_node; + + if (np && of_property_read_bool(np, "spi-3wire")) { + u8 data; + int err; + + data = LIS2DS12_ADD_INC_MASK | LIS2DS12_SIM_MASK; + err = cdata->tf->write(cdata, LIS2DS12_SIM_ADDR, 1, &data, + false); + if (err < 0) + return err; + + cdata->spi_3wire = true; + } + + return 0; +} + +int lis2ds12_common_probe(struct lis2ds12_data *cdata, int irq) +{ + u8 wai = 0; + int32_t err, i, n; + struct iio_dev *piio_dev; + struct lis2ds12_sensor_data *sdata; + + mutex_init(&cdata->regs_lock); + mutex_init(&cdata->tb.buf_lock); + mutex_init(&cdata->fifo_lock); + + cdata->fifo_data = 0; + + err = lis2ds12_init_interface(cdata); + if (err < 0) + return err; + + err = lis2ds12_read_register(cdata, LIS2DS12_WHO_AM_I_ADDR, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + if (wai != LIS2DS12_WHO_AM_I_DEF) { + dev_err(cdata->dev, "Who-Am-I value not valid (%x).\n", wai); + + return -ENODEV; + } + + cdata->hwfifo_enabled = 0; + + err = lis2ds12_set_fifo_mode(cdata, BYPASS); + if (err < 0) + return err; + + if (irq > 0) { + cdata->irq = irq; +#ifdef CONFIG_OF + err = lis2ds12_parse_dt(cdata); + if (err < 0) + return err; +#else /* CONFIG_OF */ + if (cdata->dev->platform_data) { + cdata->drdy_int_pin = ((struct lis2ds12_platform_data *) + cdata->dev->platform_data)->drdy_int_pin; + + if ((cdata->drdy_int_pin > 2) || + (cdata->drdy_int_pin < 1)) + cdata->drdy_int_pin = 1; + } else + cdata->drdy_int_pin = 1; +#endif /* CONFIG_OF */ + + dev_info(cdata->dev, "driver use DRDY int pin %d\n", + cdata->drdy_int_pin); + } + + cdata->common_odr = 0; + cdata->enabled_sensor = 0; + /* Set min watermark. */ + cdata->hwfifo_watermark = 1; + + /* + * Select sensor power mode operation. + * + * - LIS2DS12_LP_MODE: Low Power. The output data are 10 bits encoded. + * - LIS2DS12_HR_MODE: High Resolution. 14 bits output data encoding. + */ + cdata->power_mode = LIS2DS12_MODE_DEFAULT; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) { + piio_dev = + devm_iio_device_alloc(cdata->dev, + sizeof(struct lis2ds12_sensor_data *)); + if (piio_dev == NULL) { + err = -ENOMEM; + + goto iio_device_free; + } + + cdata->iio_sensors_dev[i] = piio_dev; + sdata = iio_priv(piio_dev); + sdata->enabled = false; + sdata->cdata = cdata; + sdata->sindex = i; + sdata->name = lis2ds12_sensors_table[i].name; + sdata->odr = lis2ds12_sensors_table[i].min_odr_hz; + + piio_dev->channels = lis2ds12_sensors_table[i].iio_channel; + piio_dev->num_channels = + lis2ds12_sensors_table[i].iio_channel_size; + piio_dev->info = &lis2ds12_info[i]; + piio_dev->modes = INDIO_DIRECT_MODE; + piio_dev->name = kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + sdata->name); + } + + err = lis2ds12_init_sensors(cdata); + if (err < 0) + goto iio_device_free; + + err = lis2ds12_allocate_rings(cdata); + if (err < 0) + goto iio_device_free; + + if (irq > 0) { + err = lis2ds12_allocate_triggers(cdata, LIS2DS12_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < LIS2DS12_SENSORS_NUMB; n++) { + err = iio_device_register(cdata->iio_sensors_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + dev_info(cdata->dev, "%s: probed\n", LIS2DS12_DEV_NAME); + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->iio_sensors_dev[n]); + +deallocate_ring: + lis2ds12_deallocate_rings(cdata); + +iio_device_free: + for (i--; i >= 0; i--) + iio_device_free(cdata->iio_sensors_dev[i]); + + return err; +} +EXPORT_SYMBOL(lis2ds12_common_probe); + +void lis2ds12_common_remove(struct lis2ds12_data *cdata, int irq) +{ + int i; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) + iio_device_unregister(cdata->iio_sensors_dev[i]); + + if (irq > 0) + lis2ds12_deallocate_triggers(cdata); + + lis2ds12_deallocate_rings(cdata); + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) + iio_device_free(cdata->iio_sensors_dev[i]); +} + +EXPORT_SYMBOL(lis2ds12_common_remove); + +#ifdef CONFIG_PM +int lis2ds12_common_suspend(struct lis2ds12_data *cdata) +{ + return 0; +} + +EXPORT_SYMBOL(lis2ds12_common_suspend); + +int lis2ds12_common_resume(struct lis2ds12_data *cdata) +{ + return 0; +} + +EXPORT_SYMBOL(lis2ds12_common_resume); +#endif /* CONFIG_PM */ + +MODULE_DESCRIPTION("STMicroelectronics lis2ds12 core driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2ds12_i2c.c b/drivers/iio/stm/accel/st_lis2ds12_i2c.c new file mode 100644 index 000000000000..be27f4211a51 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2ds12_i2c.c @@ -0,0 +1,181 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2ds12 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2015 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lis2ds12.h" + +static int lis2ds12_i2c_read(struct lis2ds12_data *cdata, + u8 reg_addr, int len, + u8 *data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->regs_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->regs_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int lis2ds12_i2c_write(struct lis2ds12_data *cdata, + u8 reg_addr, int len, + u8 *data, bool b_lock) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + int err; + + if (len >= LIS2DS12_TX_MAX_LENGTH) + return -ENOMEM; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = cdata->tb.tx_buf; + + if (b_lock) + mutex_lock(&cdata->regs_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->tb.buf_lock); + + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return err; +} + +static const struct lis2ds12_transfer_function lis2ds12_tf_i2c = { + .write = lis2ds12_i2c_write, + .read = lis2ds12_i2c_read, +}; + +static int lis2ds12_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct lis2ds12_data *cdata; + + cdata = kzalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + cdata->tf = &lis2ds12_tf_i2c; + i2c_set_clientdata(client, cdata); + + err = lis2ds12_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int lis2ds12_i2c_remove(struct i2c_client *client) +{ + struct lis2ds12_data *cdata = i2c_get_clientdata(client); + + lis2ds12_common_remove(cdata, client->irq); + dev_info(cdata->dev, "%s: removed\n", LIS2DS12_DEV_NAME); + kfree(cdata); + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused lis2ds12_suspend(struct device *dev) +{ + struct lis2ds12_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return lis2ds12_common_suspend(cdata); +} + +static int __maybe_unused lis2ds12_resume(struct device *dev) +{ + struct lis2ds12_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return lis2ds12_common_resume(cdata); +} + +static const struct dev_pm_ops lis2ds12_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(lis2ds12_suspend, lis2ds12_resume) +}; + +#define LIS2DS12_PM_OPS (&lis2ds12_pm_ops) +#else /* CONFIG_PM */ +#define LIS2DS12_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id lis2ds12_ids[] = { + {"lis2ds12", 0}, + {"lsm303ah", 0}, + {"lis2dg", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, lis2ds12_ids); + +#ifdef CONFIG_OF +static const struct of_device_id lis2ds12_id_table[] = { + {.compatible = "st,lis2ds12",}, + {.compatible = "st,lsm303ah",}, + {.compatible = "st,lis2dg",}, + {}, +}; + +MODULE_DEVICE_TABLE(of, lis2ds12_id_table); +#endif + +static struct i2c_driver lis2ds12_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = LIS2DS12_DEV_NAME, + .pm = LIS2DS12_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = lis2ds12_id_table, +#endif + }, + .probe = lis2ds12_i2c_probe, + .remove = lis2ds12_i2c_remove, + .id_table = lis2ds12_ids, +}; + +module_i2c_driver(lis2ds12_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics lis2ds12 i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2ds12_spi.c b/drivers/iio/stm/accel/st_lis2ds12_spi.c new file mode 100644 index 000000000000..da1a249e77fb --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2ds12_spi.c @@ -0,0 +1,196 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2ds12 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lis2ds12.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int lis2ds12_spi_read(struct lis2ds12_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->regs_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return err; +} + +static int lis2ds12_spi_write(struct lis2ds12_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= LIS2DS12_TX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->regs_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->regs_lock); + + return err; +} + +static const struct lis2ds12_transfer_function lis2ds12_tf_spi = { + .write = lis2ds12_spi_write, + .read = lis2ds12_spi_read, +}; + +static int lis2ds12_spi_probe(struct spi_device *spi) +{ + int err; + struct lis2ds12_data *cdata; + + cdata = kzalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + cdata->tf = &lis2ds12_tf_spi; + spi_set_drvdata(spi, cdata); + + err = lis2ds12_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int lis2ds12_spi_remove(struct spi_device *spi) +{ + struct lis2ds12_data *cdata = spi_get_drvdata(spi); + + lis2ds12_common_remove(cdata, spi->irq); + dev_info(cdata->dev, "%s: removed\n", LIS2DS12_DEV_NAME); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused lis2ds12_suspend(struct device *dev) +{ + struct lis2ds12_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return lis2ds12_common_suspend(cdata); +} + +static int __maybe_unused lis2ds12_resume(struct device *dev) +{ + struct lis2ds12_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return lis2ds12_common_resume(cdata); +} + +static const struct dev_pm_ops lis2ds12_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(lis2ds12_suspend, lis2ds12_resume) +}; + +#define LIS2DS12_PM_OPS (&lis2ds12_pm_ops) +#else /* CONFIG_PM */ +#define LIS2DS12_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id lis2ds12_ids[] = { + {"lis2ds12", 0}, + {"lsm303ah", 0}, + {"lis2dg", 0}, + {} +}; + +MODULE_DEVICE_TABLE(spi, lis2ds12_ids); + +#ifdef CONFIG_OF +static const struct of_device_id lis2ds12_id_table[] = { + {.compatible = "st,lis2ds12",}, + {.compatible = "st,lsm303ah",}, + {.compatible = "st,lis2dg",}, + {}, +}; + +MODULE_DEVICE_TABLE(of, lis2ds12_id_table); +#endif /* CONFIG_OF */ + +static struct spi_driver lis2ds12_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = LIS2DS12_DEV_NAME, + .pm = LIS2DS12_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = lis2ds12_id_table, +#endif /* CONFIG_OF */ + }, + .probe = lis2ds12_spi_probe, + .remove = lis2ds12_spi_remove, + .id_table = lis2ds12_ids, +}; + +module_spi_driver(lis2ds12_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics lis2ds12 spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2ds12_trigger.c b/drivers/iio/stm/accel/st_lis2ds12_trigger.c new file mode 100644 index 000000000000..4056bf4621b4 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2ds12_trigger.c @@ -0,0 +1,189 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2ds12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2015 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2ds12.h" + +static void lis2ds12_event_management(struct lis2ds12_data *cdata, u8 int_reg_val, + u8 ck_gate_val) +{ + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_TAP) && + (int_reg_val & LIS2DS12_TAP_MASK)) + iio_push_event(cdata->iio_sensors_dev[LIS2DS12_TAP], + IIO_UNMOD_EVENT_CODE(IIO_TAP, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_DOUBLE_TAP) && + (int_reg_val & LIS2DS12_DOUBLE_TAP_MASK)) + iio_push_event(cdata->iio_sensors_dev[LIS2DS12_DOUBLE_TAP], + IIO_UNMOD_EVENT_CODE(IIO_TAP_TAP, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); + + if (ck_gate_val & LIS2DS12_FUNC_CK_GATE_STEP_D_MASK) { + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_STEP_D)) + iio_push_event(cdata->iio_sensors_dev[LIS2DS12_STEP_D], + IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); + + if(CHECK_BIT(cdata->enabled_sensor, LIS2DS12_STEP_C)) + lis2ds12_read_step_c(cdata); + } + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_TILT) && + (ck_gate_val & LIS2DS12_FUNC_CK_GATE_TILT_INT_MASK)) + iio_push_event(cdata->iio_sensors_dev[LIS2DS12_TILT], + IIO_UNMOD_EVENT_CODE(IIO_TILT, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); + + if (CHECK_BIT(cdata->enabled_sensor, LIS2DS12_SIGN_M) && + (ck_gate_val & LIS2DS12_FUNC_CK_GATE_SIGN_M_DET_MASK)) + iio_push_event(cdata->iio_sensors_dev[LIS2DS12_SIGN_M], + IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, 0, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + cdata->timestamp); +} + +static inline s64 st_lis2ds12_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((LIS2DS12_EWMA_DIV - weight) * diff, LIS2DS12_EWMA_DIV); + + return old + incr; +} + +static irqreturn_t lis2ds12_irq_handler(int irq, void *private) +{ + u8 ewma_level; + struct lis2ds12_data *cdata = private; + struct iio_dev *iio_dev = dev_get_drvdata(cdata->dev); + s64 ts; + + ewma_level = (cdata->common_odr >= 100) ? 120 : 96; + ts = lis2ds12_get_time_ns(iio_dev); + cdata->accel_deltatime = st_lis2ds12_ewma(cdata->accel_deltatime, + ts - cdata->timestamp, + ewma_level); + cdata->timestamp = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t lis2ds12_irq_thread(int irq, void *private) +{ + u8 status, func; + struct lis2ds12_data *cdata = private; + + if (cdata->hwfifo_enabled) { + mutex_lock(&cdata->fifo_lock); + lis2ds12_read_fifo(cdata, true); + mutex_unlock(&cdata->fifo_lock); + } else { + cdata->tf->read(cdata, LIS2DS12_STATUS_DUP_ADDR, 1, &status, true); + if (status & (LIS2DS12_DRDY_MASK)) + lis2ds12_read_xyz(cdata); + } + + if (cdata->enabled_sensor & ~(1 << LIS2DS12_ACCEL)) { + cdata->tf->read(cdata, LIS2DS12_FUNC_CK_GATE_ADDR, 1, &func, true); + cdata->tf->read(cdata, LIS2DS12_STATUS_DUP_ADDR, 1, &status, true); + if (status & (LIS2DS12_EVENT_MASK | LIS2DS12_FUNC_CK_GATE_MASK)) + lis2ds12_event_management(cdata, status, func); + } + + return IRQ_HANDLED; +} + +int lis2ds12_allocate_triggers(struct lis2ds12_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) { +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->iio_trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->iio_sensors_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->iio_trig[i] = iio_trigger_alloc("%s-trigger", + cdata->iio_sensors_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->iio_trig[i]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + err = -ENOMEM; + + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->iio_trig[i], + cdata->iio_sensors_dev[i]); + cdata->iio_trig[i]->ops = trigger_ops; + cdata->iio_trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, + lis2ds12_irq_handler, + lis2ds12_irq_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < LIS2DS12_SENSORS_NUMB; n++) { + err = iio_trigger_register(cdata->iio_trig[n]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + + goto free_irq; + } + cdata->iio_sensors_dev[n]->trig = cdata->iio_trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->iio_trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->iio_trig[i]); + + return err; +} +EXPORT_SYMBOL(lis2ds12_allocate_triggers); + +void lis2ds12_deallocate_triggers(struct lis2ds12_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < LIS2DS12_SENSORS_NUMB; i++) + iio_trigger_unregister(cdata->iio_trig[i]); +} +EXPORT_SYMBOL(lis2ds12_deallocate_triggers); diff --git a/drivers/iio/stm/accel/st_lis2duxs12.h b/drivers/iio/stm/accel/st_lis2duxs12.h new file mode 100644 index 000000000000..899bc40de422 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12.h @@ -0,0 +1,907 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lis2duxs12 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_LIS2DUXS12_H +#define ST_LIS2DUXS12_H + +#include +#include +#include +#include +#include + +#define ST_LIS2DUXS12_ODR_EXPAND(odr, uodr) (((odr) * 1000000) + (uodr)) + +#define ST_LIS2DUX12_DEV_NAME "lis2dux12" +#define ST_LIS2DUXS12_DEV_NAME "lis2duxs12" + +/* default configuration values */ +#define ST_LIS2DUXS12_DEFAULT_WK_TH 154000 + +#define ST_LIS2DUXS12_PIN_CTRL_ADDR 0x0c +#define ST_LIS2DUXS12_PP_OD_MASK BIT(1) +#define ST_LIS2DUXS12_CS_PU_DIS_MASK BIT(2) +#define ST_LIS2DUXS12_H_LACTIVE_MASK BIT(3) +#define ST_LIS2DUXS12_PD_DIS_INT1_MASK BIT(4) +#define ST_LIS2DUXS12_PD_DIS_INT2_MASK BIT(5) +#define ST_LIS2DUXS12_SDA_PU_EN_MASK BIT(6) +#define ST_LIS2DUXS12_SDO_PU_EN_MASK BIT(7) + +#define ST_LIS2DUXS12_WHOAMI_ADDR 0x0f +#define ST_LIS2DUXS12_WHOAMI_VAL 0x47 + +#define ST_LIS2DUXS12_CTRL1_ADDR 0x10 +#define ST_LIS2DUXS12_WU_EN_MASK GENMASK(2, 0) +#define ST_LIS2DUXS12_DRDY_PULSED_MASK BIT(3) +#define ST_LIS2DUXS12_IF_ADD_INC_MASK BIT(4) +#define ST_LIS2DUXS12_SW_RESET_MASK BIT(5) +#define ST_LIS2DUXS12_INT1_ON_RES_MASK BIT(6) + +#define ST_LIS2DUXS12_CTRL2_ADDR 0x11 +#define ST_LIS2DUXS12_CTRL3_ADDR 0x12 +#define ST_LIS2DUXS12_HP_EN_MASK BIT(2) +#define ST_LIS2DUXS12_INT_FIFO_TH_MASK BIT(5) + +#define ST_LIS2DUXS12_CTRL4_ADDR 0x13 +#define ST_LIS2DUXS12_BOOT_MASK BIT(0) +#define ST_LIS2DUXS12_SOC_MASK BIT(1) +#define ST_LIS2DUXS12_FIFO_EN_MASK BIT(3) +#define ST_LIS2DUXS12_EMB_FUNC_EN_MASK BIT(4) +#define ST_LIS2DUXS12_BDU_MASK BIT(5) +#define ST_LIS2DUXS12_INACT_ODR_MASK GENMASK(7, 6) + +#define ST_LIS2DUXS12_CTRL5_ADDR 0x14 +#define ST_LIS2DUXS12_FS_MASK GENMASK(1, 0) +#define ST_LIS2DUXS12_ODR_MASK GENMASK(7, 4) + +#define ST_LIS2DUXS12_FIFO_CTRL_ADDR 0x15 +#define ST_LIS2DUXS12_FIFO_MODE_MASK GENMASK(2, 0) + +#define ST_LIS2DUXS12_FIFO_WTM_ADDR 0x16 +#define ST_LIS2DUXS12_FIFO_WTM_MASK GENMASK(6, 0) +#define ST_LIS2DUXS12_XL_ONLY_FIFO_MASK BIT(7) + +#define ST_LIS2DUXS12_INTERRUPT_CFG_ADDR 0x17 +#define ST_LIS2DUXS12_INTERRUPTS_ENABLE_MASK BIT(0) +#define ST_LIS2DUXS12_LIR_MASK BIT(1) +#define ST_LIS2DUXS12_TIMESTAMP_EN_MASK BIT(7) + +#define ST_LIS2DUXS12_SIXD_ADDR 0x18 +#define ST_LIS2DUXS12_D6D_THS_MASK GENMASK(6, 5) + +#define ST_LIS2DUXS12_WAKE_UP_THS_ADDR 0x1c +#define ST_LIS2DUXS12_WK_THS_MASK GENMASK(5, 0) +#define ST_LIS2DUXS12_SLEEP_ON_MASK BIT(6) + +#define ST_LIS2DUXS12_WAKE_UP_DUR_ADDR 0x1d +#define ST_LIS2DUXS12_SLEEP_DUR_MASK GENMASK(3, 0) +#define ST_LIS2DUXS12_WAKE_DUR_MASK GENMASK(6, 5) +#define ST_LIS2DUXS12_FF_DUR5_MASK BIT(7) + +#define ST_LIS2DUXS12_FREE_FALL_ADDR 0x1e +#define ST_LIS2DUXS12_FF_THS_MASK GENMASK(2, 0) +#define ST_LIS2DUXS12_FF_DUR_MASK GENMASK(7, 3) +#define ST_LIS2DUXS12_FF_DUR5_MASK BIT(7) + +#define ST_LIS2DUXS12_MD1_CFG_ADDR 0x1f +#define ST_LIS2DUXS12_MD2_CFG_ADDR 0x20 +#define ST_LIS2DUXS12_INT_SLEEP_CHANGE_MASK BIT(7) +#define ST_LIS2DUXS12_INT_WU_MASK BIT(5) +#define ST_LIS2DUXS12_INT_FF_MASK BIT(4) +#define ST_LIS2DUXS12_INT_TAP_MASK BIT(3) +#define ST_LIS2DUXS12_INT_6D_MASK BIT(2) +#define ST_LIS2DUXS12_INT_TIMESTAMP_MASK BIT(1) +#define ST_LIS2DUXS12_INT_EMB_FUNC_MASK BIT(0) + +#define ST_LIS2DUXS12_WAKE_UP_SRC_ADDR 0x21 +#define ST_LIS2DUXS12_WK_MASK GENMASK(2, 0) +#define ST_LIS2DUXS12_WU_IA_MASK BIT(3) +#define ST_LIS2DUXS12_SLEEP_STATE_MASK BIT(4) +#define ST_LIS2DUXS12_FF_IA_MASK BIT(5) +#define ST_LIS2DUXS12_SLEEP_CHANGE_IA_MASK BIT(6) + +#define ST_LIS2DUXS12_TAP_SRC_ADDR 0x22 +#define ST_LIS2DUXS12_TRIPLE_TAP_IA_MASK BIT(4) +#define ST_LIS2DUXS12_DOUBLE_TAP_IA_MASK BIT(5) +#define ST_LIS2DUXS12_SINGLE_TAP_IA_MASK BIT(6) +#define ST_LIS2DUXS12_TAP_IA_MASK BIT(7) + +#define ST_LIS2DUXS12_SIXD_SRC_ADDR 0x23 +#define ST_LIS2DUXS12_X_Y_Z_MASK GENMASK(5, 0) +#define ST_LIS2DUXS12_D6D_IA_MASK BIT(6) + +#define ST_LIS2DUXS12_ALL_INT_SRC_ADDR 0x24 +#define ST_LIS2DUXS12_FF_IA_ALL_MASK BIT(0) +#define ST_LIS2DUXS12_WU_IA_ALL_MASK BIT(1) +#define ST_LIS2DUXS12_SINGLE_TAP_ALL_MASK BIT(2) +#define ST_LIS2DUXS12_DOUBLE_TAP_ALL_MASK BIT(3) +#define ST_LIS2DUXS12_TRIPLE_TAP_ALL_MASK BIT(4) +#define ST_LIS2DUXS12_D6D_IA_ALL_MASK BIT(5) +#define ST_LIS2DUXS12_SLEEP_CHANGE_ALL_MASK BIT(6) + +#define ST_LIS2DUXS12_STATUS_ADDR 0x25 +#define ST_LIS2DUXS12_DRDY_MASK BIT(0) +#define ST_LIS2DUXS12_INT_GLOBAL_MASK BIT(5) + +#define ST_LIS2DUXS12_FIFO_STATUS1_ADDR 0x26 +#define ST_LIS2DUXS12_FIFO_WTM_IA_MASK BIT(7) + +#define ST_LIS2DUXS12_FIFO_STATUS2_ADDR 0x27 +#define ST_LIS2DUXS12_FIFO_FSS_MASK GENMASK(7, 0) + +#define ST_LIS2DUXS12_OUT_X_L_ADDR 0x28 +#define ST_LIS2DUXS12_OUT_Y_L_ADDR 0x2a +#define ST_LIS2DUXS12_OUT_Z_L_ADDR 0x2c +#define ST_LIS2DUXS12_OUT_T_L_ADDR 0x2e + +#define ST_LIS2DUXS12_AH_QVAR_CFG_ADDR 0x31 +#define ST_LIS2DUXS12_AH_QVAR_EN_MASK BIT(7) +#define ST_LIS2DUXS12_AH_QVAR_NOTCH_EN_MASK BIT(6) +#define ST_LIS2DUXS12_AH_QVAR_NOTCH_CUTOFF_MASK BIT(5) +#define ST_LIS2DUXS12_AH_QVAR_C_ZIN_MASK GENMASK(4, 3) +#define ST_LIS2DUXS12_AH_QVAR_GAIN_MASK GENMASK(2, 1) + +#define ST_LIS2DUXS12_SELF_TEST_ADDR 0x32 +#define ST_LIS2DUXS12_ST_MASK GENMASK(5, 4) + +#define ST_LIS2DUXS12_EMB_FUNC_STATUS_MAINPAGE_ADDR 0x34 +#define ST_LIS2DUXS12_IS_STEP_DET_MASK BIT(3) +#define ST_LIS2DUXS12_IS_TILT_MASK BIT(4) +#define ST_LIS2DUXS12_IS_SIGMOT_MASK BIT(5) + +#define ST_LIS2DUXS12_FSM_STATUS_MAINPAGE_ADDR 0x35 +#define ST_LIS2DUXS12_MLC_STATUS_MAINPAGE_ADDR 0x36 + +#define ST_LIS2DUXS12_FUNC_CFG_ACCESS_ADDR 0x3f +#define ST_LIS2DUXS12_EMB_FUNC_REG_ACCESS_MASK BIT(7) +#define ST_LIS2DUXS12_FSM_WR_CTRL_EN_MASK BIT(0) + +#define ST_LIS2DUXS12_FIFO_DATA_OUT_TAG_ADDR 0x40 + +#define ST_LIS2DUXS12_FIFO_BATCH_DEC_ADDR 0x47 +#define ST_LIS2DUXS12_BDR_XL_MASK GENMASK(2, 0) +#define ST_LIS2DUXS12_DEC_TS_MASK GENMASK(4, 3) + +#define ST_LIS2DUXS12_TAP_CFG0_ADDR 0x6f +#define ST_LIS2DUXS12_AXIS_MASK GENMASK(7, 6) +#define ST_LIS2DUXS12_INVERT_T_MASK GENMASK(5, 0) + +#define ST_LIS2DUXS12_TAP_CFG1_ADDR 0x70 +#define ST_LIS2DUXS12_POST_STILL_T_MASK GENMASK(3, 0) +#define ST_LIS2DUXS12_PRE_STILL_THS_MASK GENMASK(7, 4) + +#define ST_LIS2DUXS12_TAP_CFG2_ADDR 0x71 +#define ST_LIS2DUXS12_WAIT_T_MASK GENMASK(5, 0) +#define ST_LIS2DUXS12_POST_STILL_TH_MASK GENMASK(7, 6) + +#define ST_LIS2DUXS12_TAP_CFG3_ADDR 0x72 +#define ST_LIS2DUXS12_LATENCY_T_MASK GENMASK(3, 0) +#define ST_LIS2DUXS12_POST_STILL_THS_MASK GENMASK(7, 4) + +#define ST_LIS2DUXS12_TAP_CFG4_ADDR 0x73 +#define ST_LIS2DUXS12_PEAK_THS_MASK GENMASK(5, 0) +#define ST_LIS2DUXS12_WAIT_END_LATENCY_MASK BIT(7) + +#define ST_LIS2DUXS12_TAP_CFG5_ADDR 0x74 +#define ST_LIS2DUXS12_REBOUND_T_MASK GENMASK(4, 0) +#define ST_LIS2DUXS12_SINGLE_TAP_EN_MASK BIT(5) +#define ST_LIS2DUXS12_DOUBLE_TAP_EN_MASK BIT(6) +#define ST_LIS2DUXS12_TRIPLE_TAP_EN_MASK BIT(7) + +#define ST_LIS2DUXS12_TAP_CFG6_ADDR 0x75 +#define ST_LIS2DUXS12_PRE_STILL_N_MASK GENMASK(3, 0) +#define ST_LIS2DUXS12_PRE_STILL_ST_MASK GENMASK(7, 4) + +#define ST_LIS2DUXS12_TIMESTAMP2_ADDR 0x7c + +#define ST_LIS2DUXS12_SELFTEST_ACCEL_MIN 204 +#define ST_LIS2DUXS12_SELFTEST_ACCEL_MAX 4918 + +/* embedded function registers */ +#define ST_LIS2DUXS12_PAGE_SEL_ADDR 0x02 +#define ST_LIS2DUXS12_EMB_FUNC_EN_A_ADDR 0x04 +#define ST_LIS2DUXS12_PEDO_EN_MASK BIT(3) +#define ST_LIS2DUXS12_TILT_EN_MASK BIT(4) +#define ST_LIS2DUXS12_SIGN_MOTION_EN_MASK BIT(5) +#define ST_LIS2DUXS12_MLC_BEFORE_FSM_EN_MASK BIT(7) + +#define ST_LIS2DUXS12_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_LIS2DUXS12_FSM_EN_MASK BIT(0) +#define ST_LIS2DUXS12_MLC_EN_MASK BIT(4) + +#define ST_LIS2DUXS12_EMB_FUNC_EXEC_STATUS_ADDR 0x07 +#define ST_LIS2DUXS12_FUNC_ENDOP_MASK BIT(0) +#define ST_LIS2DUXS12_FUNC_EXEC_OVR_MASK BIT(1) + +#define ST_LIS2DUXS12_PAGE_ADDRESS_ADDR 0x08 +#define ST_LIS2DUXS12_PAGE_VALUE_ADDR 0x09 + +#define ST_LIS2DUXS12_EMB_FUNC_INT1_ADDR 0x0a +#define ST_LIS2DUXS12_INT_STEP_DETECTOR_MASK BIT(3) +#define ST_LIS2DUXS12_INT_TILT_MASK BIT(4) +#define ST_LIS2DUXS12_INT_SIG_MOT_MASK BIT(5) + +#define ST_LIS2DUXS12_FSM_INT1_ADDR 0x0b +#define ST_LIS2DUXS12_MLC_INT1_ADDR 0x0d +#define ST_LIS2DUXS12_EMB_FUNC_INT2_ADDR 0x0e +#define ST_LIS2DUXS12_FSM_INT2_ADDR 0x0f +#define ST_LIS2DUXS12_MLC_INT2_ADDR 0x11 + +#define ST_LIS2DUXS12_EMB_FUNC_STATUS_ADDR 0x12 +#define ST_LIS2DUXS12_EMB_IS_STEP_DET_MASK BIT(3) +#define ST_LIS2DUXS12_EMB_IS_TILT_MASK BIT(4) +#define ST_LIS2DUXS12_EMB_IS_SIGMOT_MASK BIT(5) + +#define ST_LIS2DUXS12_FSM_STATUS_ADDR 0x13 +#define ST_LIS2DUXS12_MLC_STATUS_ADDR 0x15 + +#define ST_LIS2DUXS12_PAGE_RW_ADDR 0x17 +#define ST_LIS2DUXS12_EMB_FUNC_LIR_MASK BIT(7) + +#define ST_LIS2DUXS12_EMB_FUNC_FIFO_EN_ADDR 0x18 +#define ST_LIS2DUXS12_STEP_COUNTER_FIFO_EN_MASK BIT(0) +#define ST_LIS2DUXS12_MLC_FIFO_EN_MASK BIT(1) +#define ST_LIS2DUXS12_MLC_FILTER_FEATURE_FIFO_EN_MASK BIT(2) +#define ST_LIS2DUXS12_FSM_FIFO_EN_MASK BIT(3) + +#define ST_LIS2DUXS12_FSM_ENABLE_ADDR 0x1a +#define ST_LIS2DUXS12_FSM_OUTS1_ADDR 0x20 + +#define ST_LIS2DUXS12_STEP_COUNTER_L_ADDR 0x28 +#define ST_LIS2DUXS12_STEP_COUNTER_H_ADDR 0x29 + +#define ST_LIS2DUXS12_EMB_FUNC_SRC_ADDR 0x2a +#define ST_LIS2DUXS12_STEP_DETECTED_MASK BIT(7) +#define ST_LIS2DUXS12_PEDO_RST_STEP_MASK BIT(8) + +#define ST_LIS2DUXS12_EMB_FUNC_INIT_A_ADDR 0x2c +#define ST_LIS2DUXS12_STEP_DET_INIT_MASK BIT(3) +#define ST_LIS2DUXS12_TILT_INIT_MASK BIT(4) +#define ST_LIS2DUXS12_SIG_MOT_INIT_MASK BIT(5) +#define ST_LIS2DUXS12_MLC_BEFORE_FSM_INIT_MASK BIT(7) + +#define ST_LIS2DUXS12_EMB_FUNC_INIT_B_ADDR 0x2d +#define ST_LIS2DUXS12_FSM_INIT_MASK BIT(0) +#define ST_LIS2DUXS12_MLC_INIT_MASK BIT(4) + +#define ST_LIS2DUXS12_MLC1_SRC_ADDR 0x34 + +#define ST_LIS2DUXS12_FSM_ODR_ADDR 0x39 +#define ST_LIS2DUXS12_FSM_ODR_MASK GENMASK(5, 3) + +#define ST_LIS2DUXS12_MLC_ODR_ADDR 0x3a +#define ST_LIS2DUXS12_MLC_ODR_MASK GENMASK(6, 4) + +/* Timestamp Tick 10us/LSB */ +#define ST_LIS2DUXS12_TS_DELTA_NS 10000ULL + +/* Temperature in uC */ +#define ST_LIS2DUXS12_TEMP_GAIN 256 +#define ST_LIS2DUXS12_TEMP_OFFSET 6400 + +/* FIFO simple size and depth */ +#define ST_LIS2DUXS12_SAMPLE_SIZE 6 +#define ST_LIS2DUXS12_TS_SAMPLE_SIZE 4 +#define ST_LIS2DUXS12_TAG_SIZE 1 +#define ST_LIS2DUXS12_FIFO_SAMPLE_SIZE (ST_LIS2DUXS12_SAMPLE_SIZE + \ + ST_LIS2DUXS12_TAG_SIZE) +#define ST_LIS2DUXS12_MAX_FIFO_DEPTH 127 + +struct __packed raw_data_compact_t { + __le16 x:12; + __le16 y:12; + __le16 z:12; + __le16 t:12; +}; + +struct __packed raw_data_t { + __le16 x; + __le16 y; + __le16 z; +}; + +#define ST_LIS2DUXS12_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, \ + .shift = sb - rb, \ + .endianness = IIO_LE, \ + }, \ + .ext_info = ext_inf, \ +} + +static const struct iio_event_spec st_lis2duxs12_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_lis2duxs12_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_LIS2DUXS12_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lis2duxs12_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_LIS2DUXS12_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) +#define ST_LIS2DUXS12_DESHIFT_VAL(val, mask) (((val) & (mask)) >> __ffs(mask)) + +enum st_lis2duxs12_pm_t { + ST_LIS2DUXS12_LP_MODE = 0, + ST_LIS2DUXS12_HP_MODE, + ST_LIS2DUXS12_NO_MODE, +}; + +enum st_lis2duxs12_fsm_mlc_enable_id { + ST_LIS2DUXS12_MLC_FSM_DISABLED = 0, + ST_LIS2DUXS12_MLC_ENABLED = BIT(0), + ST_LIS2DUXS12_FSM_ENABLED = BIT(1), +}; + +/** + * struct mlc_config_t - + * @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. + * @requested_device: Device bitmask requested by firmware. + * @status: MLC / FSM enabled status. + */ +struct st_lis2duxs12_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; + uint32_t requested_device; + enum st_lis2duxs12_fsm_mlc_enable_id status; +}; + +/** + * struct st_lis2duxs12_ff_th - Free Fall threshold table + * @mg: Threshold in mg. + * @val: Register value. + */ +struct st_lis2duxs12_ff_th { + u32 mg; + u8 val; +}; + +/** + * struct st_lis2duxs12_6D_th - 6D threshold table + * @deg: Threshold in degrees. + * @val: Register value. + */ +struct st_lis2duxs12_6D_th { + u8 deg; + u8 val; +}; + +/** + * struct st_lis2duxs12_reg - Generic sensor register description addr + + * mask + * @addr: Address of register. + * @mask: Bitmask register for proper usage. + */ +struct st_lis2duxs12_reg { + u8 addr; + u8 mask; +}; + +/** + * Define embedded functions register access + * + * FUNC_CFG_ACCESS_0 is default bank + * FUNC_CFG_ACCESS_FUNC_CFG Enable access to the embedded functions + * configuration registers. + */ +enum st_lis2duxs12_page_sel_register { + FUNC_CFG_ACCESS_0 = 0, + FUNC_CFG_ACCESS_FUNC_CFG, +}; + +/** + * struct st_lis2duxs12_odr - Single ODR entry + * @hz: Most significant part of the sensor ODR (Hz). + * @uhz: Less significant part of the sensor ODR (micro Hz). + * @val: ODR register value. + */ +struct st_lis2duxs12_odr { + u16 hz; + u32 uhz; + u8 val; +}; + +/** + * struct st_lis2duxs12_odr_table_entry - Sensor ODR table + * @size: Size of ODR table. + * @reg: ODR register. + * @pm: Power mode register. + * @batching_reg: ODR register for batching on fifo. + * @odr_avl: Array of supported ODR value. + */ +struct st_lis2duxs12_odr_table_entry { + u8 size; + struct st_lis2duxs12_reg reg; + struct st_lis2duxs12_reg pm; + struct st_lis2duxs12_odr odr_avl[10]; +}; + +/** + * struct st_lis2duxs12_fs - Full Scale sensor table entry + * @gain: Sensor sensitivity (mdps/LSB, mg/LSB and uC/LSB). + * @val: FS register value. + */ +struct st_lis2duxs12_fs { + u32 gain; + u8 val; +}; + +/** + * struct st_lis2duxs12_fs_table_entry - Full Scale sensor table + * @size: Full Scale sensor table size. + * @reg: Register description for FS settings. + * @fs_avl: Full Scale list entries. + */ +struct st_lis2duxs12_fs_table_entry { + u8 size; + struct st_lis2duxs12_reg reg; + struct st_lis2duxs12_fs fs_avl[4]; +}; + +enum st_lis2duxs12_sensor_id { + ST_LIS2DUXS12_ID_ACC = 0, + ST_LIS2DUXS12_ID_TEMP, + ST_LIS2DUXS12_ID_STEP_COUNTER, + ST_LIS2DUXS12_ID_STEP_DETECTOR, + ST_LIS2DUXS12_ID_SIGN_MOTION, + ST_LIS2DUXS12_ID_TILT, + ST_LIS2DUXS12_ID_QVAR, + ST_LIS2DUXS12_ID_FF, + ST_LIS2DUXS12_ID_SC, + ST_LIS2DUXS12_ID_WK, + ST_LIS2DUXS12_ID_6D, + ST_LIS2DUXS12_ID_TAP, + ST_LIS2DUXS12_ID_DTAP, + ST_LIS2DUXS12_ID_TTAP, + ST_LIS2DUXS12_ID_MLC, + ST_LIS2DUXS12_ID_MLC_0, + ST_LIS2DUXS12_ID_MLC_1, + ST_LIS2DUXS12_ID_MLC_2, + ST_LIS2DUXS12_ID_MLC_3, + ST_LIS2DUXS12_ID_FSM_0, + ST_LIS2DUXS12_ID_FSM_1, + ST_LIS2DUXS12_ID_FSM_2, + ST_LIS2DUXS12_ID_FSM_3, + ST_LIS2DUXS12_ID_FSM_4, + ST_LIS2DUXS12_ID_FSM_5, + ST_LIS2DUXS12_ID_FSM_6, + ST_LIS2DUXS12_ID_FSM_7, + ST_LIS2DUXS12_ID_MAX, +}; + +static const enum +st_lis2duxs12_sensor_id st_lis2duxs12_buffered_sensor_list[] = { + [0] = ST_LIS2DUXS12_ID_ACC, + [1] = ST_LIS2DUXS12_ID_TEMP, + [2] = ST_LIS2DUXS12_ID_STEP_COUNTER, + [3] = ST_LIS2DUXS12_ID_QVAR, +}; + +#define ST_LIS2DUXS12_BUFFERED_ENABLED (BIT(ST_LIS2DUXS12_ID_ACC) | \ + BIT(ST_LIS2DUXS12_ID_TEMP) | \ + BIT(ST_LIS2DUXS12_ID_STEP_COUNTER) | \ + BIT(ST_LIS2DUXS12_ID_QVAR)) + +static const enum +st_lis2duxs12_sensor_id st_lis2duxs12_mlc_sensor_list[] = { + [0] = ST_LIS2DUXS12_ID_MLC_0, + [1] = ST_LIS2DUXS12_ID_MLC_1, + [2] = ST_LIS2DUXS12_ID_MLC_2, + [3] = ST_LIS2DUXS12_ID_MLC_3, +}; + +static const enum +st_lis2duxs12_sensor_id st_lis2duxs12_fsm_sensor_list[] = { + [0] = ST_LIS2DUXS12_ID_FSM_0, + [1] = ST_LIS2DUXS12_ID_FSM_1, + [2] = ST_LIS2DUXS12_ID_FSM_2, + [3] = ST_LIS2DUXS12_ID_FSM_3, + [4] = ST_LIS2DUXS12_ID_FSM_4, + [5] = ST_LIS2DUXS12_ID_FSM_5, + [6] = ST_LIS2DUXS12_ID_FSM_6, + [7] = ST_LIS2DUXS12_ID_FSM_7, +}; + +#define ST_LIS2DUXS12_EMB_FUNC_ENABLED (BIT(ST_LIS2DUXS12_ID_STEP_DETECTOR) | \ + BIT(ST_LIS2DUXS12_ID_SIGN_MOTION) | \ + BIT(ST_LIS2DUXS12_ID_TILT)) + +#define ST_LIS2DUXS12_BASIC_FUNC_ENABLED (GENMASK(ST_LIS2DUXS12_ID_TTAP, \ + ST_LIS2DUXS12_ID_FF)) + +/* HW devices that can wakeup the target */ +#define ST_LIS2DUXS12_WAKE_UP_SENSORS (BIT(ST_LIS2DUXS12_ID_ACC) | \ + BIT(ST_LIS2DUXS12_ID_MLC_0) | \ + BIT(ST_LIS2DUXS12_ID_MLC_1) | \ + BIT(ST_LIS2DUXS12_ID_MLC_2) | \ + BIT(ST_LIS2DUXS12_ID_MLC_3) | \ + BIT(ST_LIS2DUXS12_ID_FSM_0) | \ + BIT(ST_LIS2DUXS12_ID_FSM_1) | \ + BIT(ST_LIS2DUXS12_ID_FSM_2) | \ + BIT(ST_LIS2DUXS12_ID_FSM_3) | \ + BIT(ST_LIS2DUXS12_ID_FSM_4) | \ + BIT(ST_LIS2DUXS12_ID_FSM_5) | \ + BIT(ST_LIS2DUXS12_ID_FSM_6) | \ + BIT(ST_LIS2DUXS12_ID_FSM_7)) + +/* this is the minimal ODR for wake-up sensors and dependencies */ +#define ST_LIS2DUXS12_MIN_ODR_IN_WAKEUP 25 + +enum st_lis2duxs12_fifo_mode { + ST_LIS2DUXS12_FIFO_BYPASS = 0x0, + ST_LIS2DUXS12_FIFO_CONT = 0x6, +}; + +enum { + ST_LIS2DUXS12_HW_FLUSH, + ST_LIS2DUXS12_HW_OPERATIONAL, +}; + +enum st_lis2duxs12_hw_id { + ST_LIS2DUX12_ID, + ST_LIS2DUXS12_ID, + ST_LIS2DUXS12_MAX_ID, +}; + +/** + * struct st_lis2duxs12_settings - ST IMU sensor settings + * + * @hw_id: Hw id supported by the driver configuration. + * @name: Device name supported by the driver configuration. + * @st_qvar_support: QVAR supported flag. + */ +struct st_lis2duxs12_settings { + struct { + enum st_lis2duxs12_hw_id hw_id; + const char *name; + } id; + bool st_qvar_support; +}; +/** + * struct st_lis2duxs12_sensor - ST ACC sensor instance + */ +struct st_lis2duxs12_sensor { + char name[32]; + enum st_lis2duxs12_sensor_id id; + struct st_lis2duxs12_hw *hw; + struct iio_trigger *trig; + + int odr; + int uodr; + + union { + /* sensor with odrs, gain and offset */ + struct { + u32 gain; + u32 offset; + u8 decimator; + u8 dec_counter; + __le16 old_data; + u8 max_watermark; + u8 watermark; + enum st_lis2duxs12_pm_t pm; + + /* self test */ + int8_t selftest_status; + int min_st; + int max_st; + }; + /* mlc/fsm event sensors */ + struct { + uint8_t status_reg; + uint8_t outreg_addr; + enum st_lis2duxs12_fsm_mlc_enable_id status; + }; + /* sensor specific data configuration */ + struct { + u32 conf[6]; + /* Ensure natural alignment of timestamp */ + struct { + u8 event; + s64 ts __aligned(8); + } scan; + }; + }; +}; + +/** + * struct st_lis2duxs12_hw - ST ACC MEMS hw instance + */ +struct st_lis2duxs12_hw { + struct device *dev; + int irq; + struct regmap *regmap; + struct mutex page_lock; + struct mutex fifo_lock; + enum st_lis2duxs12_fifo_mode fifo_mode; + unsigned long state; + bool xl_only; + bool timestamp; + + u8 std_level; + u64 samples; + + u32 enable_mask; + u32 requested_mask; + + s64 ts_offset; + s64 hw_ts; + s64 tsample; + s64 delta_ts; + s64 ts; + s64 last_fifo_timestamp; + + struct iio_mount_matrix orientation; + struct regulator *vdd_supply; + struct regulator *vddio_supply; + + struct st_lis2duxs12_mlc_config_t *mlc_config; + const struct st_lis2duxs12_odr_table_entry *odr_table_entry; + const struct st_lis2duxs12_fs_table_entry *fs_table_entry; + + bool preload_mlc; + + u8 int_pin; + u8 ft_int_reg; + u8 md_int_reg; + u8 emb_int_reg; + + struct iio_dev *iio_devs[ST_LIS2DUXS12_ID_MAX]; + const struct st_lis2duxs12_settings *settings; +}; + +extern const struct dev_pm_ops st_lis2duxs12_pm_ops; + +static inline bool +st_lis2duxs12_is_fifo_enabled(struct st_lis2duxs12_hw *hw) +{ + return hw->enable_mask & (BIT(ST_LIS2DUXS12_ID_ACC) | + BIT(ST_LIS2DUXS12_ID_TEMP)); +} + +static inline int +__st_lis2duxs12_write_with_mask(struct st_lis2duxs12_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_LIS2DUXS12_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int +st_lis2duxs12_update_bits_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_lis2duxs12_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +/* use when mask is constant */ +static inline int +st_lis2duxs12_write_with_mask_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = FIELD_PREP(mask, data); + + mutex_lock(&hw->page_lock); + err = regmap_update_bits(hw->regmap, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lis2duxs12_read_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, void *val, + unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lis2duxs12_read_with_mask(struct st_lis2duxs12_hw *hw, + unsigned int addr, unsigned int mask, + u8 *val) +{ + unsigned int data; + int err; + + err = regmap_read(hw->regmap, addr, &data); + *val = (u8)ST_LIS2DUXS12_DESHIFT_VAL(data, mask); + + return err; +} + +static inline int +st_lis2duxs12_read_with_mask_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, + unsigned int mask, u8 *val) +{ + unsigned int data; + int err; + + mutex_lock(&hw->page_lock); + err = regmap_read(hw->regmap, addr, &data); + mutex_unlock(&hw->page_lock); + *val = (u8)ST_LIS2DUXS12_DESHIFT_VAL(data, mask); + + return err; +} + +static inline int +st_lis2duxs12_write_locked(struct st_lis2duxs12_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_lis2duxs12_set_emb_access(struct st_lis2duxs12_hw *hw, + unsigned int val) +{ + return regmap_write(hw->regmap, + ST_LIS2DUXS12_FUNC_CFG_ACCESS_ADDR, + val ? ST_LIS2DUXS12_EMB_FUNC_REG_ACCESS_MASK : 0); +} + +static inline int +st_lis2duxs12_read_page_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, void *val, + unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lis2duxs12_set_emb_access(hw, 1); + err = regmap_bulk_read(hw->regmap, addr, val, len); + st_lis2duxs12_set_emb_access(hw, 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lis2duxs12_write_page_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, unsigned int *val, + unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lis2duxs12_set_emb_access(hw, 1); + err = regmap_bulk_write(hw->regmap, addr, val, len); + st_lis2duxs12_set_emb_access(hw, 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lis2duxs12_update_page_bits_locked(struct st_lis2duxs12_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lis2duxs12_set_emb_access(hw, 1); + err = regmap_update_bits(hw->regmap, addr, mask, val); + st_lis2duxs12_set_emb_access(hw, 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +int st_lis2duxs12_probe(struct device *dev, int irq, + enum st_lis2duxs12_hw_id hw_id, struct regmap *regmap); +int st_lis2duxs12_remove(struct device *dev); +int st_lis2duxs12_sensor_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable); +int st_lis2duxs12_buffers_setup(struct st_lis2duxs12_hw *hw); +ssize_t st_lis2duxs12_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lis2duxs12_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lis2duxs12_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lis2duxs12_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +int st_lis2duxs12_suspend_fifo(struct st_lis2duxs12_hw *hw); +int st_lis2duxs12_set_fifo_mode(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_fifo_mode fifo_mode); +int st_lis2duxs12_update_batching(struct iio_dev *iio_dev, bool enable); + +/* mlc / fsm */ +int st_lis2duxs12_mlc_probe(struct st_lis2duxs12_hw *hw); +int st_lis2duxs12_mlc_remove(struct device *dev); +int st_lis2duxs12_mlc_check_status(struct st_lis2duxs12_hw *hw); +int st_lis2duxs12_mlc_init_preload(struct st_lis2duxs12_hw *hw); + +int st_lis2duxs12_reset_step_counter(struct iio_dev *iio_dev); +int st_lis2duxs12_embedded_function_init(struct st_lis2duxs12_hw *hw); +int st_lis2duxs12_step_counter_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable); +int st_lis2duxs12_embfunc_sensor_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable); +int st_lis2duxs12_probe_basicfunc(struct st_lis2duxs12_hw *hw); +int st_lis2duxs12_event_handler(struct st_lis2duxs12_hw *hw); + +/* qvar */ +int st_lis2duxs12_qvar_probe(struct st_lis2duxs12_hw *hw); +int st_lis2duxs12_qvar_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable); +#endif /* ST_LIS2DUXS12_H */ diff --git a/drivers/iio/stm/accel/st_lis2duxs12_basicfunc.c b/drivers/iio/stm/accel/st_lis2duxs12_basicfunc.c new file mode 100644 index 000000000000..f32f8d67f47a --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_basicfunc.c @@ -0,0 +1,1065 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 basic function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2duxs12.h" + +static const unsigned long st_lis2duxs12_event_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static const struct iio_chan_spec st_lis2duxs12_wk_channels[] = { + { + .type = 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_lis2duxs12_ff_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_GESTURE, thr), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct iio_chan_spec st_lis2duxs12_tap_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_TAP, thr), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct iio_chan_spec st_lis2duxs12_dtap_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_TAP_TAP, thr), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct iio_chan_spec st_lis2duxs12_ttap_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_GESTURE, thr), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct iio_chan_spec st_lis2duxs12_6D_channels[] = { + { + .type = 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_lis2duxs12_sleepchange_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_GESTURE, thr), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct st_lis2duxs12_ff_th st_lis2duxs12_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_lis2duxs12_6D_th st_lis2duxs12_6D_threshold[] = { + [0] = { + .val = 0x00, + .deg = 80, + }, + [1] = { + .val = 0x01, + .deg = 70, + }, + [2] = { + .val = 0x02, + .deg = 60, + }, + [3] = { + .val = 0x03, + .deg = 50, + }, +}; + +/* + * st_lis2duxs12_set_wake_up_thershold - set wake-up threshold in ug + * @hw - ST ACC MEMS hw instance + * @th_ug - wake-up threshold in ug (micro g) + * + * wake-up thershold (th_umss) is expressed in micro m/s^2, register + * val is (th_umss * 2^6) / (1000000 * FS_XL(m/s^2)) + */ +static int +st_lis2duxs12_set_wake_up_thershold(struct st_lis2duxs12_hw *hw, + int th_umss) +{ + struct st_lis2duxs12_sensor *sensor; + struct iio_dev *iio_dev; + u8 val, max_th, fs_xl; + int tmp, err, i; + + err = st_lis2duxs12_read_with_mask_locked(hw, + hw->fs_table_entry[ST_LIS2DUXS12_ID_ACC].reg.addr, + hw->fs_table_entry[ST_LIS2DUXS12_ID_ACC].reg.mask, + &fs_xl); + if (err < 0) + return err; + + for (i = 0; i < hw->fs_table_entry->size; i++) { + if (hw->fs_table_entry->fs_avl[i].val == fs_xl) + break; + } + + if (i == hw->fs_table_entry->size) + return -EINVAL; + + tmp = (th_umss * 64) / + (hw->fs_table_entry->fs_avl[i].gain * 16384); + val = (u8)tmp; + max_th = ST_LIS2DUXS12_WK_THS_MASK >> ffs(ST_LIS2DUXS12_WK_THS_MASK); + if (val > max_th) + val = max_th; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_WAKE_UP_THS_ADDR, + ST_LIS2DUXS12_WK_THS_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[0] = th_umss; + + return 0; +} + +/* + * st_lis2duxs12_set_wake_up_duration - set wake-up duration in ms + * @hw - ST ACC MEMS hw instance + * @dur_ms - wake-up duration in ms + * + * wake-up duration register val is related to XL ODR + */ +static int +st_lis2duxs12_set_wake_up_duration(struct st_lis2duxs12_hw *hw, + int dur_ms) +{ + struct st_lis2duxs12_sensor *sensor; + struct iio_dev *iio_dev; + int i, tmp, sensor_odr, err; + u8 val, max_dur, odr_xl; + + err = st_lis2duxs12_read_with_mask_locked(hw, + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].reg.addr, + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].reg.mask, + &odr_xl); + if (err < 0) + return err; + + if (odr_xl == 0) { + dev_info(hw->dev, "use default ODR\n"); + odr_xl = hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[4].val; + } + + for (i = 0; i < hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].size; i++) { + if (odr_xl == + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].val) + break; + } + + if (i == hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].size) + return -EINVAL; + + sensor_odr = ST_LIS2DUXS12_ODR_EXPAND( + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].hz, + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].uhz); + + tmp = dur_ms / (1000000 / (sensor_odr / 1000)); + val = (u8)tmp; + max_dur = ST_LIS2DUXS12_WAKE_DUR_MASK >> ffs(ST_LIS2DUXS12_WAKE_DUR_MASK); + if (val > max_dur) + val = max_dur; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_WAKE_UP_DUR_ADDR, + ST_LIS2DUXS12_WAKE_DUR_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[1] = dur_ms; + + return 0; +} + +/* + * st_lis2duxs12_set_freefall_threshold - set free fall threshold detection mg + * @hw - ST ACC MEMS hw instance + * @th_mg - free fall threshold in mg + */ +static int +st_lis2duxs12_set_freefall_threshold(struct st_lis2duxs12_hw *hw, + int th_mg) +{ + struct st_lis2duxs12_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_free_fall_threshold); i++) { + if (th_mg >= st_lis2duxs12_free_fall_threshold[i].mg) + break; + } + + if (i == ARRAY_SIZE(st_lis2duxs12_free_fall_threshold)) + return -EINVAL; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_FREE_FALL_ADDR, + ST_LIS2DUXS12_FF_THS_MASK, + st_lis2duxs12_free_fall_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_FF]; + sensor = iio_priv(iio_dev); + sensor->conf[2] = th_mg; + + return 0; +} + +/* + * st_lis2duxs12_set_6D_threshold - set 6D threshold detection in degrees + * @hw - ST ACC MEMS hw instance + * @deg - 6D threshold in degrees + */ +static int st_lis2duxs12_set_6D_threshold(struct st_lis2duxs12_hw *hw, + int deg) +{ + struct st_lis2duxs12_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_6D_threshold); i++) { + if (deg >= st_lis2duxs12_6D_threshold[i].deg) + break; + } + + if (i == ARRAY_SIZE(st_lis2duxs12_6D_threshold)) + return -EINVAL; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_SIXD_ADDR, + ST_LIS2DUXS12_D6D_THS_MASK, + st_lis2duxs12_6D_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_6D]; + sensor = iio_priv(iio_dev); + sensor->conf[3] = deg; + + return 0; +} + +static int +st_lis2duxs12_event_sensor_enable(struct st_lis2duxs12_sensor *sensor, + bool enable) +{ + struct st_lis2duxs12_hw *hw = sensor->hw; + int err, eint = !!enable; + + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + switch (sensor->id) { + case ST_LIS2DUXS12_ID_WK: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_WU_MASK, + eint); + if (err < 0) + return err; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_CTRL1_ADDR, + ST_LIS2DUXS12_WU_EN_MASK, + eint ? 0x07 : 0); + if (err < 0) + return err; + break; + case ST_LIS2DUXS12_ID_FF: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_FF_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LIS2DUXS12_ID_SC: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_SLEEP_CHANGE_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LIS2DUXS12_ID_6D: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_6D_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LIS2DUXS12_ID_TAP: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_TAP_CFG5_ADDR, + ST_LIS2DUXS12_SINGLE_TAP_EN_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LIS2DUXS12_ID_DTAP: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_TAP_CFG5_ADDR, + ST_LIS2DUXS12_DOUBLE_TAP_EN_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LIS2DUXS12_ID_TTAP: + err = st_lis2duxs12_update_bits_locked(hw, + hw->md_int_reg, + ST_LIS2DUXS12_INT_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_TAP_CFG5_ADDR, + ST_LIS2DUXS12_TRIPLE_TAP_EN_MASK, + eint); + if (err < 0) + return err; + break; + default: + err = -EINVAL; + break; + } + + if (err >= 0) { + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_INTERRUPT_CFG_ADDR, + ST_LIS2DUXS12_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_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +static int +st_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_lis2duxs12_event_sensor_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +ssize_t st_lis2duxs12_wakeup_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[0]); +} + +ssize_t st_lis2duxs12_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_lis2duxs12_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_lis2duxs12_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; +} + +ssize_t st_lis2duxs12_wakeup_duration_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[1]); +} + +ssize_t +st_lis2duxs12_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_lis2duxs12_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_lis2duxs12_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; +} + +ssize_t st_lis2duxs12_freefall_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[2]); +} + +ssize_t st_lis2duxs12_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_lis2duxs12_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_lis2duxs12_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; +} + +ssize_t st_lis2duxs12_6D_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[3]); +} + +ssize_t st_lis2duxs12_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_lis2duxs12_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_lis2duxs12_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_lis2duxs12_wakeup_threshold_get, + st_lis2duxs12_wakeup_threshold_set, 0); + +static IIO_DEVICE_ATTR(wakeup_duration, 0644, + st_lis2duxs12_wakeup_duration_get, + st_lis2duxs12_wakeup_duration_set, 0); + +static IIO_DEVICE_ATTR(freefall_threshold, 0644, + st_lis2duxs12_freefall_threshold_get, + st_lis2duxs12_freefall_threshold_set, 0); + +static IIO_DEVICE_ATTR(sixd_threshold, 0644, + st_lis2duxs12_6D_threshold_get, + st_lis2duxs12_6D_threshold_set, 0); + +static struct attribute *st_lis2duxs12_wk_attributes[] = { + &iio_dev_attr_wakeup_threshold.dev_attr.attr, + &iio_dev_attr_wakeup_duration.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis2duxs12_wk_attribute_group = { + .attrs = st_lis2duxs12_wk_attributes, +}; + +static const struct iio_info st_lis2duxs12_wk_info = { + .attrs = &st_lis2duxs12_wk_attribute_group, +}; + +static struct attribute *st_lis2duxs12_ff_attributes[] = { + &iio_dev_attr_freefall_threshold.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis2duxs12_ff_attribute_group = { + .attrs = st_lis2duxs12_ff_attributes, +}; + +static const struct iio_info st_lis2duxs12_ff_info = { + .attrs = &st_lis2duxs12_ff_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct attribute *st_lis2duxs12_sc_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2duxs12_sc_attribute_group = { + .attrs = st_lis2duxs12_sc_attributes, +}; + +static const struct iio_info st_lis2duxs12_sc_info = { + .attrs = &st_lis2duxs12_sc_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct attribute *st_lis2duxs12_6D_attributes[] = { + &iio_dev_attr_sixd_threshold.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis2duxs12_6D_attribute_group = { + .attrs = st_lis2duxs12_6D_attributes, +}; + +static const struct iio_info st_lis2duxs12_6D_info = { + .attrs = &st_lis2duxs12_6D_attribute_group, +}; + +static struct attribute *st_lis2duxs12_tap_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2duxs12_tap_attribute_group = { + .attrs = st_lis2duxs12_tap_attributes, +}; + +static const struct iio_info st_lis2duxs12_tap_info = { + .attrs = &st_lis2duxs12_tap_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct attribute *st_lis2duxs12_dtap_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2duxs12_dtap_attribute_group = { + .attrs = st_lis2duxs12_dtap_attributes, +}; + +static const struct iio_info st_lis2duxs12_dtap_info = { + .attrs = &st_lis2duxs12_dtap_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct attribute *st_lis2duxs12_ttap_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2duxs12_ttap_attribute_group = { + .attrs = st_lis2duxs12_ttap_attributes, +}; + +static const struct iio_info st_lis2duxs12_ttap_info = { + .attrs = &st_lis2duxs12_ttap_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct iio_dev * +st_lis2duxs12_alloc_event_iiodev(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_sensor_id id) +{ + struct st_lis2duxs12_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_lis2duxs12_event_available_scan_masks; + + switch (id) { + case ST_LIS2DUXS12_ID_WK: + iio_dev->channels = st_lis2duxs12_wk_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_wk_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_wk", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_wk_info; + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_FF: + iio_dev->channels = st_lis2duxs12_ff_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_ff_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ff", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_ff_info; + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_SC: + iio_dev->channels = st_lis2duxs12_sleepchange_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_sleepchange_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sc", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_sc_info; + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_6D: + iio_dev->channels = st_lis2duxs12_6D_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_6D_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_6d", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_6D_info; + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_TAP: + iio_dev->channels = st_lis2duxs12_tap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_tap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tap", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_tap_info; + /* request ODR @400 Hz to works properly */ + sensor->odr = 400; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_DTAP: + iio_dev->channels = st_lis2duxs12_dtap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_dtap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_dtap", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_dtap_info; + /* request ODR @400 Hz to works properly */ + sensor->odr = 400; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_TTAP: + iio_dev->channels = st_lis2duxs12_ttap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_ttap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ttap", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_ttap_info; + /* request ODR @400 Hz to works properly */ + sensor->odr = 400; + sensor->uodr = 0; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +int st_lis2duxs12_event_handler(struct st_lis2duxs12_hw *hw) +{ + struct iio_dev *iio_dev; + int status; + s64 event; + int err; + + if (hw->enable_mask & ST_LIS2DUXS12_BASIC_FUNC_ENABLED) { + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_ALL_INT_SRC_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + if (status & ST_LIS2DUXS12_FF_IA_ALL_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_FF]; + event = IIO_UNMOD_EVENT_CODE(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_LIS2DUXS12_WU_IA_ALL_MASK) { + struct st_lis2duxs12_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_WK]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + if (status & ST_LIS2DUXS12_SLEEP_CHANGE_ALL_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_SC]; + event = IIO_UNMOD_EVENT_CODE(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_LIS2DUXS12_D6D_IA_ALL_MASK) { + struct st_lis2duxs12_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_6D]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + if (status & ST_LIS2DUXS12_SINGLE_TAP_ALL_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_TAP]; + event = IIO_UNMOD_EVENT_CODE(IIO_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_LIS2DUXS12_DOUBLE_TAP_ALL_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_DTAP]; + event = IIO_UNMOD_EVENT_CODE(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_LIS2DUXS12_TRIPLE_TAP_ALL_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_TTAP]; + /* triple tap is not available as IIO type */ + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + } + + return IRQ_HANDLED; +} + +static inline int st_lis2duxs12_get_6D(struct st_lis2duxs12_hw *hw, + u8 *out) +{ + return st_lis2duxs12_read_with_mask_locked(hw, + ST_LIS2DUXS12_SIXD_SRC_ADDR, + ST_LIS2DUXS12_X_Y_Z_MASK, + out); +} + +static inline int st_lis2duxs12_get_wk(struct st_lis2duxs12_hw *hw, + u8 *out) +{ + return st_lis2duxs12_read_with_mask_locked(hw, + ST_LIS2DUXS12_WAKE_UP_SRC_ADDR, + ST_LIS2DUXS12_WK_MASK, out); +} + +static irqreturn_t st_lis2duxs12_6D_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + + st_lis2duxs12_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_lis2duxs12_wk_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + + st_lis2duxs12_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; +} + +static const struct iio_trigger_ops st_lis2duxs12_trigger_ops = { + NULL, +}; + +static int st_lis2duxs12_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lis2duxs12_event_sensor_enable(iio_priv(iio_dev), + true); +} + +static int st_lis2duxs12_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lis2duxs12_event_sensor_enable(iio_priv(iio_dev), + false); +} + +static const struct iio_buffer_setup_ops st_lis2duxs12_buffer_ops = { + .preenable = st_lis2duxs12_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_lis2duxs12_buffer_postdisable, +}; + +/* + * st_lis2duxs12_init_tap - initialize tap configuration + * + * This section can be customized + */ +static int st_lis2duxs12_init_tap(struct st_lis2duxs12_hw *hw) +{ + int err; + + err = regmap_write(hw->regmap, + ST_LIS2DUXS12_TAP_CFG0_ADDR, + 0xc8); + if (err) + return err; + + err = regmap_write(hw->regmap, + ST_LIS2DUXS12_TAP_CFG1_ADDR, + 0x28); + if (err) + return err; + + err = regmap_write(hw->regmap, + ST_LIS2DUXS12_TAP_CFG2_ADDR, + 0x03); + if (err) + return err; + + err = regmap_write(hw->regmap, + ST_LIS2DUXS12_TAP_CFG3_ADDR, + 0x84); + if (err) + return err; + + err = regmap_write(hw->regmap, + ST_LIS2DUXS12_TAP_CFG4_ADDR, + 0x08); + if (err) + return err; + + err = regmap_write(hw->regmap, + ST_LIS2DUXS12_TAP_CFG6_ADDR, + 0x0a); + if (err) + return err; + + return 0; +} + +int st_lis2duxs12_probe_basicfunc(struct st_lis2duxs12_hw *hw) +{ + struct st_lis2duxs12_sensor *sensor; + struct iio_dev *iio_dev; + irqreturn_t (*pthread[2])(int irq, void *p) = { + [0] = st_lis2duxs12_wk_handler_thread, + [1] = st_lis2duxs12_6D_handler_thread, + }; + int i, err; + + for (i = ST_LIS2DUXS12_ID_FF; + i <= ST_LIS2DUXS12_ID_TTAP; i++) { + hw->iio_devs[i] = st_lis2duxs12_alloc_event_iiodev(hw, + i); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + /* configure trigger sensors */ + for (i = ST_LIS2DUXS12_ID_WK; i <= ST_LIS2DUXS12_ID_6D; i++) { + iio_dev = hw->iio_devs[i]; + + err = devm_iio_triggered_buffer_setup(hw->dev, iio_dev, + NULL, pthread[i - ST_LIS2DUXS12_ID_WK], + &st_lis2duxs12_buffer_ops); + if (err < 0) + return err; + + sensor = iio_priv(iio_dev); + sensor->trig = devm_iio_trigger_alloc(hw->dev, + "%s-trigger", + iio_dev->name); + if (!sensor->trig) + return -ENOMEM; + + iio_trigger_set_drvdata(sensor->trig, iio_dev); + sensor->trig->ops = &st_lis2duxs12_trigger_ops; + sensor->trig->dev.parent = hw->dev; + iio_dev->trig = iio_trigger_get(sensor->trig); + + err = devm_iio_trigger_register(hw->dev, sensor->trig); + if (err) + return err; + } + + err = st_lis2duxs12_init_tap(hw); + if (err) + return err; + + /* set default settings threshold */ + return st_lis2duxs12_set_wake_up_thershold(hw, + ST_LIS2DUXS12_DEFAULT_WK_TH); +} diff --git a/drivers/iio/stm/accel/st_lis2duxs12_buffer.c b/drivers/iio/stm/accel/st_lis2duxs12_buffer.c new file mode 100644 index 000000000000..12ad69d5f3c0 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_buffer.c @@ -0,0 +1,751 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2duxs12.h" + +/* Timestamp convergence filter parameters */ +#define ST_LIS2DUXS12_EWMA_LEVEL 120 +#define ST_LIS2DUXS12_EWMA_DIV 128 + +/* FIFO tags */ +enum { + ST_LIS2DUXS12_ACC_TEMP_TAG = 0x02, + ST_LIS2DUXS12_TS_TAG = 0x04, + ST_LIS2DUXS12_STEP_COUNTER_TAG = 0x12, + ST_LIS2DUXS12_ACC_QVAR_TAG = 0x1f, +}; + +static inline s64 st_lis2duxs12_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LIS2DUXS12_EWMA_DIV - weight) * diff, + ST_LIS2DUXS12_EWMA_DIV); + + return old + incr; +} + +static inline int st_lis2duxs12_reset_hwts(struct st_lis2duxs12_hw *hw) +{ + u8 data = 0xaa; + + hw->ts = iio_get_time_ns(hw->iio_devs[0]); + hw->ts_offset = hw->ts; + hw->tsample = 0ull; + + return st_lis2duxs12_write_locked(hw, ST_LIS2DUXS12_TIMESTAMP2_ADDR, + data); +} + +int st_lis2duxs12_set_fifo_mode(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_fifo_mode fifo_mode) +{ + int err; + + err = st_lis2duxs12_write_with_mask_locked(hw, + ST_LIS2DUXS12_FIFO_CTRL_ADDR, + ST_LIS2DUXS12_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +static int st_lis2duxs12_update_watermark(struct st_lis2duxs12_sensor *sensor, + u8 watermark) +{ + u8 fifo_watermark = ST_LIS2DUXS12_MAX_FIFO_DEPTH; + struct st_lis2duxs12_hw *hw = sensor->hw; + struct st_lis2duxs12_sensor *cur_sensor; + u8 cur_watermark = 0; + int err; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_buffered_sensor_list); + i++) { + enum st_lis2duxs12_sensor_id id = + st_lis2duxs12_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(u8, fifo_watermark, + cur_watermark); + } + + fifo_watermark = max_t(u8, fifo_watermark, + hw->timestamp ? 2 : 1); + + err = st_lis2duxs12_write_with_mask_locked(hw, + ST_LIS2DUXS12_FIFO_WTM_ADDR, + ST_LIS2DUXS12_FIFO_WTM_MASK, + fifo_watermark); + + return err; +} + +static int st_lis2duxs12_read_fifo(struct st_lis2duxs12_hw *hw) +{ + u8 iio_buf[ALIGN(ST_LIS2DUXS12_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)]; + u8 buf[6 * ST_LIS2DUXS12_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + u8 fifo_status, fifo_depth; + s64 ts_irq, hw_ts_old; + u32 val; + + if (hw->fifo_mode == ST_LIS2DUXS12_FIFO_BYPASS) + return 0; + + ts_irq = hw->ts -hw->delta_ts; + + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_FIFO_STATUS2_ADDR, + &fifo_status, + sizeof(fifo_status)); + if (err < 0) + return err; + + fifo_depth = fifo_status & ST_LIS2DUXS12_FIFO_FSS_MASK; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_LIS2DUXS12_FIFO_SAMPLE_SIZE; + read_len = 0; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_FIFO_DATA_OUT_TAG_ADDR, + buf, word_len); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_LIS2DUXS12_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_LIS2DUXS12_TAG_SIZE]; + tag = buf[i] >> 3; + + switch (tag) { + case ST_LIS2DUXS12_TS_TAG: + val = get_unaligned_le32(ptr + 2); + hw_ts_old = hw->hw_ts; + hw->hw_ts = val * ST_LIS2DUXS12_TS_DELTA_NS; + hw->ts_offset = + st_lis2duxs12_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_LIS2DUXS12_EWMA_LEVEL); + 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; + break; + case ST_LIS2DUXS12_ACC_TEMP_TAG: { + struct iio_dev *iio_dev = + hw->iio_devs[ST_LIS2DUXS12_ID_ACC]; + + if (hw->timestamp) + hw->tsample = min_t(s64, + iio_get_time_ns(iio_dev), + hw->tsample); + else + hw->tsample = iio_get_time_ns(iio_dev); + + hw->last_fifo_timestamp = hw->tsample; + + if (hw->xl_only) { + /* + * data representation in FIFO + * when ACC only: + * ----------- ----------- + * | LSBX | MSBX | + * ----------- ----------- + * | LSBY | MSBY | + * ----------- ----------- + * | LSBZ | MSBZ | + * ----------- ----------- + */ + memcpy(iio_buf, + ptr, ST_LIS2DUXS12_SAMPLE_SIZE); + if (unlikely(++hw->samples < hw->std_level)) + continue; + + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, hw->tsample); + } else { + struct raw_data_compact_t *raw_data_c; + struct iio_dev *iio_temp_dev; + struct raw_data_t raw_data; + __le16 temp; + + raw_data_c = (struct raw_data_compact_t *)ptr; + iio_temp_dev = + hw->iio_devs[ST_LIS2DUXS12_ID_TEMP]; + + /* + * data representation in FIFO + * when ACC/Temp available: + * ------------- ------------- + * | LSB0 | LSN1 | MSN0 | + * ------------- ------------- + * | MSB1 | LSB2 | + * ------------- ------------- + * | LSN3 | MSN2 | MSB3 | + * ------------- ------------- + */ + + /* extends to 16 bit */ + temp = cpu_to_le16(le16_to_cpu(raw_data_c->t) << 4); + + memcpy(iio_buf, (u8 *)&temp, sizeof(temp)); + iio_push_to_buffers_with_timestamp(iio_temp_dev, + iio_buf, hw->tsample); + + if (unlikely(++hw->samples < hw->std_level)) + continue; + + /* extends to 16 bit */ + raw_data.x = cpu_to_le16(le16_to_cpu(raw_data_c->x) << 4); + raw_data.y = cpu_to_le16(le16_to_cpu(raw_data_c->y) << 4); + raw_data.z = cpu_to_le16(le16_to_cpu(raw_data_c->z) << 4); + + memcpy(iio_buf, (u8 *)&raw_data, sizeof(raw_data)); + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, hw->tsample); + } + break; + } + case ST_LIS2DUXS12_ACC_QVAR_TAG: { + struct iio_dev *iio_dev = + hw->iio_devs[ST_LIS2DUXS12_ID_ACC]; + struct raw_data_compact_t *raw_data_c; + struct iio_dev *iio_qvar_dev; + struct raw_data_t raw_data; + __le16 qvar; + + if (hw->timestamp) + hw->tsample = min_t(s64, + iio_get_time_ns(iio_dev), + hw->tsample); + else + hw->tsample = iio_get_time_ns(iio_dev); + + hw->last_fifo_timestamp = hw->tsample; + + raw_data_c = (struct raw_data_compact_t *)ptr; + iio_qvar_dev = + hw->iio_devs[ST_LIS2DUXS12_ID_QVAR]; + + /* + * data representation in FIFO + * when ACC/Qvar available: + * ------------- ------------- + * | LSB0 | LSN1 | MSN0 | + * ------------- ------------- + * | MSB1 | LSB2 | + * ------------- ------------- + * | LSN3 | MSN2 | MSB3 | + * ------------- ------------- + */ + + /* extends to 16 bit */ + qvar = cpu_to_le16(le16_to_cpu(raw_data_c->t) << 4); + + memcpy(iio_buf, (u8 *)&qvar, sizeof(qvar)); + iio_push_to_buffers_with_timestamp(iio_qvar_dev, + iio_buf, + hw->tsample); + + /* skip push acc if not enabled */ + if (!(hw->enable_mask & BIT(ST_LIS2DUXS12_ID_ACC)) || + unlikely(++hw->samples < hw->std_level)) + continue; + + /* extends to 16 bit */ + raw_data.x = cpu_to_le16(le16_to_cpu(raw_data_c->x) << 4); + raw_data.y = cpu_to_le16(le16_to_cpu(raw_data_c->y) << 4); + raw_data.z = cpu_to_le16(le16_to_cpu(raw_data_c->z) << 4); + + memcpy(iio_buf, (u8 *)&raw_data, sizeof(raw_data)); + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + break; + } + case ST_LIS2DUXS12_STEP_COUNTER_TAG: { + struct iio_dev *iio_dev = + hw->iio_devs[ST_LIS2DUXS12_ID_STEP_COUNTER]; + + val = get_unaligned_le32(ptr + 2); + hw->tsample = val * ST_LIS2DUXS12_TS_DELTA_NS; + memcpy(iio_buf, ptr, ST_LIS2DUXS12_SAMPLE_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + break; + } + default: + break; + } + } + + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lis2duxs12_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +ssize_t st_lis2duxs12_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +ssize_t st_lis2duxs12_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_lis2duxs12_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_lis2duxs12_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_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + s64 type, event, fts, ts; + int count; + + mutex_lock(&hw->fifo_lock); + ts = iio_get_time_ns(iio_dev); + hw->delta_ts = ts -hw->ts; + hw->ts = ts; + set_bit(ST_LIS2DUXS12_HW_FLUSH, &hw->state); + count = st_lis2duxs12_read_fifo(hw); + sensor->dec_counter = 0; + if (count > 0) + fts = hw->last_fifo_timestamp; + else + fts = ts; + mutex_unlock(&hw->fifo_lock); + + type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fts); + + return size; +} + +int st_lis2duxs12_suspend_fifo(struct st_lis2duxs12_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + st_lis2duxs12_read_fifo(hw); + err = st_lis2duxs12_set_fifo_mode(hw, ST_LIS2DUXS12_FIFO_BYPASS); + mutex_unlock(&hw->fifo_lock); + + return err; +} + +static int st_lis2duxs12_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + mutex_lock(&hw->fifo_lock); + + switch (sensor->id) { + case ST_LIS2DUXS12_ID_QVAR: { + u8 xl_only = enable ? 0 : 1; + + /* + * check consistency, temperature sensor need to be + * disabled because share the same QVAR output registers + */ + if (hw->enable_mask & BIT(ST_LIS2DUXS12_ID_TEMP)) { + err = -EBUSY; + + goto out; + } + + err = st_lis2duxs12_qvar_set_enable(sensor, enable); + if (err < 0) + goto out; + + /* enable XL and Temp */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_FIFO_WTM_ADDR, + ST_LIS2DUXS12_XL_ONLY_FIFO_MASK, + FIELD_PREP(ST_LIS2DUXS12_XL_ONLY_FIFO_MASK, + xl_only)); + if (err < 0) + goto out; + + hw->xl_only = !!xl_only; + break; + } + case ST_LIS2DUXS12_ID_TEMP: { + u8 xl_only = enable ? 0 : 1; + + /* + * check consistency, QVAR sensor need to be disabled + * because share the same TEMP output registers + */ + if (hw->enable_mask & BIT(ST_LIS2DUXS12_ID_QVAR)) { + err = -EBUSY; + + goto out; + } + + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + + /* enable XL and Temp */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_FIFO_WTM_ADDR, + ST_LIS2DUXS12_XL_ONLY_FIFO_MASK, + FIELD_PREP(ST_LIS2DUXS12_XL_ONLY_FIFO_MASK, + xl_only)); + if (err < 0) + goto out; + + hw->xl_only = !!xl_only; + break; + } + case ST_LIS2DUXS12_ID_STEP_COUNTER: + err = st_lis2duxs12_step_counter_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LIS2DUXS12_ID_ACC: + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + default: + break; + } + + err = st_lis2duxs12_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (enable && hw->fifo_mode == ST_LIS2DUXS12_FIFO_BYPASS) { + st_lis2duxs12_reset_hwts(hw); + err = st_lis2duxs12_set_fifo_mode(hw, + ST_LIS2DUXS12_FIFO_CONT); + } else if (!(hw->enable_mask & ST_LIS2DUXS12_BUFFERED_ENABLED)) { + err = st_lis2duxs12_set_fifo_mode(hw, + ST_LIS2DUXS12_FIFO_BYPASS); + } + +out: + mutex_unlock(&hw->fifo_lock); + enable_irq(hw->irq); + + return err; +} + +static irqreturn_t st_lis2duxs12_handler_irq(int irq, void *private) +{ + struct st_lis2duxs12_hw *hw = (struct st_lis2duxs12_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_lis2duxs12_handler_thread(int irq, void *private) +{ + struct st_lis2duxs12_hw *hw = (struct st_lis2duxs12_hw *)private; + + st_lis2duxs12_mlc_check_status(hw); + + mutex_lock(&hw->fifo_lock); + st_lis2duxs12_read_fifo(hw); + clear_bit(ST_LIS2DUXS12_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + + if (hw->enable_mask & (BIT(ST_LIS2DUXS12_ID_STEP_DETECTOR) | + BIT(ST_LIS2DUXS12_ID_TILT) | + BIT(ST_LIS2DUXS12_ID_SIGN_MOTION))) { + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_EMB_FUNC_STATUS_MAINPAGE_ADDR, + &status, sizeof(status)); + if (err < 0) + goto out; + + /* embedded function sensors */ + if (status & ST_LIS2DUXS12_IS_STEP_DET_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_STEP_DETECTOR]; + event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LIS2DUXS12_IS_SIGMOT_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_SIGN_MOTION]; + event = IIO_UNMOD_EVENT_CODE(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_LIS2DUXS12_IS_TILT_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DUXS12_ID_TILT]; + event = IIO_UNMOD_EVENT_CODE(IIO_TILT, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + } + +out: + return st_lis2duxs12_event_handler(hw); +} + +static int st_lis2duxs12_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_lis2duxs12_update_fifo(iio_dev, true); +} + +static int st_lis2duxs12_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_lis2duxs12_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lis2duxs12_fifo_ops = { + .preenable = st_lis2duxs12_fifo_preenable, + .postdisable = st_lis2duxs12_fifo_postdisable, +}; + +int st_lis2duxs12_buffers_setup(struct st_lis2duxs12_hw *hw) +{ + struct device_node *np = hw->dev->of_node; + unsigned long irq_type; + bool irq_active_low; + int err; + int i; + + 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; + } + + /* configure interrupt pin level */ + if (irq_active_low) { + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_PIN_CTRL_ADDR, + ST_LIS2DUXS12_H_LACTIVE_MASK, + FIELD_PREP(ST_LIS2DUXS12_H_LACTIVE_MASK, 1)); + if (err < 0) + return err; + } + + if (np && of_property_read_bool(np, "drive-open-drain")) { + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_PIN_CTRL_ADDR, + ST_LIS2DUXS12_PP_OD_MASK, + FIELD_PREP(ST_LIS2DUXS12_PP_OD_MASK, 1)); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + /* check pull down disable on int1 pin property */ + if (np && of_property_read_bool(np, "pd_dis_int1")) { + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_PIN_CTRL_ADDR, + ST_LIS2DUXS12_PD_DIS_INT1_MASK, + FIELD_PREP(ST_LIS2DUXS12_PD_DIS_INT1_MASK, 1)); + if (err < 0) + return err; + } + + if (hw->settings->st_qvar_support) { + if (hw->int_pin == 1) { + /* + * route on RES pin the interrupt pin configured when + * qvar supported + */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_CTRL1_ADDR, + ST_LIS2DUXS12_INT1_ON_RES_MASK, + FIELD_PREP(ST_LIS2DUXS12_INT1_ON_RES_MASK, 1)); + if (err < 0) + return err; + } else { + dev_err(hw->dev, + "if qvar enabled only irq pin 1 can be used\n"); + + return err; + } + } else { + /* + * check pull down disable on int2 pin property (not + * supported when qvar enabled) + */ + if (np && of_property_read_bool(np, "pd_dis_int2")) { + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_PIN_CTRL_ADDR, + ST_LIS2DUXS12_PD_DIS_INT2_MASK, + FIELD_PREP(ST_LIS2DUXS12_PD_DIS_INT2_MASK, 1)); + if (err < 0) + return err; + } + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lis2duxs12_handler_irq, + st_lis2duxs12_handler_thread, + irq_type | IRQF_ONESHOT, + ST_LIS2DUXS12_DEV_NAME, hw); + if (err) { + dev_err(hw->dev, + "failed to request trigger irq %d\n", + hw->irq); + + return err; + } + + /* allocate buffer for all buffered sensor type */ + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_buffered_sensor_list); i++) { + enum st_lis2duxs12_sensor_id id = + st_lis2duxs12_buffered_sensor_list[i]; + +#if KERNEL_VERSION(5, 13, 0) > LINUX_VERSION_CODE + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + + if (!hw->iio_devs[id]) + continue; + +#if KERNEL_VERSION(5, 13, 0) <= LINUX_VERSION_CODE + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[id], + INDIO_BUFFER_SOFTWARE, + &st_lis2duxs12_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[id], buffer); + hw->iio_devs[id]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[id]->setup_ops = &st_lis2duxs12_fifo_ops; +#endif /* LINUX_VERSION_CODE */ + } + + if (hw->timestamp) { + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_FIFO_BATCH_DEC_ADDR, + ST_LIS2DUXS12_DEC_TS_MASK, + FIELD_PREP(ST_LIS2DUXS12_DEC_TS_MASK, 1)); + if (err < 0) + return err; + } + + return 0; +} diff --git a/drivers/iio/stm/accel/st_lis2duxs12_core.c b/drivers/iio/stm/accel/st_lis2duxs12_core.c new file mode 100644 index 000000000000..a2382ca0202a --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_core.c @@ -0,0 +1,1705 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lis2duxs12.h" + +static const struct st_lis2duxs12_std_entry { + u16 odr; + u8 val; +} st_lis2duxs12_std_table[] = { + { 1, 1 }, + { 3, 1 }, + { 6, 2 }, + { 12, 3 }, + { 25, 3 }, + { 50, 3 }, + { 100, 3 }, + { 200, 3 }, + { 400, 3 }, + { 800, 3 }, +}; + +static struct st_lis2duxs12_selftest_table { + char *string_mode; + u8 mode; +} st_lis2duxs12_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .mode = 0, + }, + [1] = { + .string_mode = "positive-sign", + .mode = 1, + }, + [2] = { + .string_mode = "negative-sign", + .mode = 2, + }, +}; + +static struct st_lis2duxs12_power_mode_table { + char *string_mode; + enum st_lis2duxs12_pm_t val; +} st_lis2duxs12_power_mode[] = { + [ST_LIS2DUXS12_LP_MODE] = { + .string_mode = "LP_MODE", + .val = ST_LIS2DUXS12_LP_MODE, + }, + [ST_LIS2DUXS12_HP_MODE] = { + .string_mode = "HP_MODE", + .val = ST_LIS2DUXS12_HP_MODE, + }, +}; + +static const struct +st_lis2duxs12_odr_table_entry st_lis2duxs12_odr_table[] = { + [ST_LIS2DUXS12_ID_ACC] = { + .size = 10, + .reg = { + .addr = ST_LIS2DUXS12_CTRL5_ADDR, + .mask = ST_LIS2DUXS12_ODR_MASK, + }, + .pm = { + .addr = ST_LIS2DUXS12_CTRL3_ADDR, + .mask = ST_LIS2DUXS12_HP_EN_MASK, + }, + .odr_avl[0] = { 1, 600000, 0x01 }, + .odr_avl[1] = { 3, 0, 0x02 }, + .odr_avl[2] = { 6, 0, 0x04 }, + .odr_avl[3] = { 12, 500000, 0x05 }, + .odr_avl[4] = { 25, 0, 0x06 }, + .odr_avl[5] = { 50, 0, 0x07 }, + .odr_avl[6] = { 100, 0, 0x08 }, + .odr_avl[7] = { 200, 0, 0x09 }, + .odr_avl[8] = { 400, 0, 0x0a }, + .odr_avl[9] = { 800, 0, 0x0b }, + }, + [ST_LIS2DUXS12_ID_TEMP] = { + .size = 10, + .reg = { + .addr = ST_LIS2DUXS12_CTRL5_ADDR, + .mask = ST_LIS2DUXS12_ODR_MASK, + }, + .odr_avl[0] = { 1, 600000, 0x01 }, + .odr_avl[1] = { 3, 0, 0x02 }, + .odr_avl[2] = { 6, 0, 0x04 }, + .odr_avl[3] = { 12, 500000, 0x05 }, + .odr_avl[4] = { 25, 0, 0x06 }, + .odr_avl[5] = { 50, 0, 0x07 }, + .odr_avl[6] = { 100, 0, 0x08 }, + .odr_avl[7] = { 200, 0, 0x09 }, + .odr_avl[8] = { 400, 0, 0x0a }, + .odr_avl[9] = { 800, 0, 0x0b }, + }, +}; + +static const struct +st_lis2duxs12_fs_table_entry st_lis2duxs12_fs_table[] = { + [ST_LIS2DUXS12_ID_ACC] = { + .size = 4, + .reg = { + .addr = ST_LIS2DUXS12_CTRL5_ADDR, + .mask = ST_LIS2DUXS12_FS_MASK, + }, + .fs_avl[0] = { + .gain = IIO_G_TO_M_S_2(61), + .val = 0x0, + }, + .fs_avl[1] = { + .gain = IIO_G_TO_M_S_2(122), + .val = 0x1, + }, + .fs_avl[2] = { + .gain = IIO_G_TO_M_S_2(244), + .val = 0x2, + }, + .fs_avl[3] = { + .gain = IIO_G_TO_M_S_2(488), + .val = 0x3, + }, + }, + [ST_LIS2DUXS12_ID_TEMP] = { + .size = 1, + .fs_avl[0] = { + .gain = (1000000 / ST_LIS2DUXS12_TEMP_GAIN), + .val = 0x0 + }, + }, +}; + +/** + * List of supported device settings + * + * The following table list all device features in terms of supported + * features. + */ +static const struct st_lis2duxs12_settings st_lis2duxs12_sensor_settings[] = { + { + .id = { + .hw_id = ST_LIS2DUX12_ID, + .name = ST_LIS2DUX12_DEV_NAME, + }, + }, + { + .id = { + .hw_id = ST_LIS2DUXS12_ID, + .name = ST_LIS2DUXS12_DEV_NAME, + }, + .st_qvar_support = true, + }, +}; + +static const struct iio_mount_matrix * +st_lis2duxs12_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *ch) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct +iio_chan_spec_ext_info st_lis2duxs12_chan_spec_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, + st_lis2duxs12_get_mount_matrix), + {} +}; + +static const struct iio_chan_spec st_lis2duxs12_acc_channels[] = { + ST_LIS2DUXS12_DATA_CHANNEL(IIO_ACCEL, + ST_LIS2DUXS12_OUT_X_L_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lis2duxs12_chan_spec_ext_info), + ST_LIS2DUXS12_DATA_CHANNEL(IIO_ACCEL, + ST_LIS2DUXS12_OUT_Y_L_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lis2duxs12_chan_spec_ext_info), + ST_LIS2DUXS12_DATA_CHANNEL(IIO_ACCEL, + ST_LIS2DUXS12_OUT_Z_L_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lis2duxs12_chan_spec_ext_info), + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lis2duxs12_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LIS2DUXS12_OUT_T_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_LIS2DUXS12_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct +iio_chan_spec st_lis2duxs12_step_counter_channels[] = { + { + .type = IIO_STEP_COUNTER, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_STEP_COUNTER, flush), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static const struct +iio_chan_spec st_lis2duxs12_step_detector_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_STEP_DETECTOR, thr), +}; + +static const struct +iio_chan_spec st_lis2duxs12_sign_motion_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_SIGN_MOTION, thr), +}; + +static const struct iio_chan_spec st_lis2duxs12_tilt_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_TILT, thr), +}; + +static const unsigned long st_lis2duxs12_available_scan_masks[] = { + GENMASK(2, 0), 0x0 +}; + +static const unsigned long st_lis2duxs12_temp_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static const unsigned long st_lis2duxs12_emb_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static inline int +st_lis2duxs12_set_std_level(struct st_lis2duxs12_hw *hw, u16 odr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_std_table); i++) { + if (st_lis2duxs12_std_table[i].odr >= odr) + break; + } + + if (i == ARRAY_SIZE(st_lis2duxs12_std_table)) + return -EINVAL; + + hw->std_level = st_lis2duxs12_std_table[i].val; + + return 0; +} + +static __maybe_unused int +st_lis2duxs12_reg_access(struct iio_dev *iio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval) +{ + struct st_lis2duxs12_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_lis2duxs12_power_up_command(struct st_lis2duxs12_hw *hw) +{ + int data; + + regmap_read(hw->regmap, ST_LIS2DUXS12_WHOAMI_ADDR, &data); + + usleep_range(25000, 26000); + + return data; +} + +static int st_lis2duxs12_check_whoami(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_hw_id hw_id) +{ + int data, err, i; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_sensor_settings); i++) { + if (st_lis2duxs12_sensor_settings[i].id.name && + st_lis2duxs12_sensor_settings[i].id.hw_id == hw_id) + break; + } + + if (i == ARRAY_SIZE(st_lis2duxs12_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", hw_id); + + return -ENODEV; + } + + err = st_lis2duxs12_set_emb_access(hw, false); + if (err < 0) + return err; + + err = regmap_read(hw->regmap, ST_LIS2DUXS12_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_LIS2DUXS12_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + hw->settings = &st_lis2duxs12_sensor_settings[i]; + + return 0; +} + +static int +st_lis2duxs12_set_full_scale(struct st_lis2duxs12_sensor *sensor, + u32 gain) +{ + enum st_lis2duxs12_sensor_id id = sensor->id; + struct st_lis2duxs12_hw *hw = sensor->hw; + int i, err; + u8 val; + + for (i = 0; i < st_lis2duxs12_fs_table[id].size; i++) + if (st_lis2duxs12_fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == st_lis2duxs12_fs_table[id].size) + return -EINVAL; + + val = st_lis2duxs12_fs_table[id].fs_avl[i].val; + err = regmap_update_bits(hw->regmap, + st_lis2duxs12_fs_table[id].reg.addr, + st_lis2duxs12_fs_table[id].reg.mask, + ST_LIS2DUXS12_SHIFT_VAL(val, + st_lis2duxs12_fs_table[id].reg.mask)); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lis2duxs12_get_odr_val(enum st_lis2duxs12_sensor_id id, + int odr, int uodr, + struct st_lis2duxs12_odr *oe) +{ + int req_odr = ST_LIS2DUXS12_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < st_lis2duxs12_odr_table[id].size; i++) { + sensor_odr = ST_LIS2DUXS12_ODR_EXPAND( + st_lis2duxs12_odr_table[id].odr_avl[i].hz, + st_lis2duxs12_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= req_odr) { + oe->hz = st_lis2duxs12_odr_table[id].odr_avl[i].hz; + oe->uhz = st_lis2duxs12_odr_table[id].odr_avl[i].uhz; + oe->val = st_lis2duxs12_odr_table[id].odr_avl[i].val; + + return 0; + } + } + + return -EINVAL; +} + +static u16 +st_lis2duxs12_check_odr_dependency(struct st_lis2duxs12_hw *hw, + int odr, int uodr, + enum st_lis2duxs12_sensor_id ref_id) +{ + struct st_lis2duxs12_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = odr > 0; + u16 ret; + + if (enable) { + /* uodr not used */ + if (hw->enable_mask & BIT(ref_id)) + ret = max_t(u16, ref->odr, odr); + else + ret = odr; + } else { + ret = (hw->enable_mask & BIT(ref_id)) ? ref->odr : 0; + } + + return ret; +} + +static int st_lis2duxs12_set_odr(struct st_lis2duxs12_sensor *sensor, + int req_odr, int req_uodr) +{ + enum st_lis2duxs12_sensor_id id = ST_LIS2DUXS12_ID_ACC; + struct st_lis2duxs12_hw *hw = sensor->hw; + struct st_lis2duxs12_odr oe = { 0 }; + enum st_lis2duxs12_sensor_id i; + int err, odr; + + for (i = ST_LIS2DUXS12_ID_ACC; i < ST_LIS2DUXS12_ID_MAX; i++) { + if (!hw->iio_devs[i] || i == sensor->id) + continue; + + odr = st_lis2duxs12_check_odr_dependency(hw, req_odr, + req_uodr, i); + if (odr != req_odr) + return 0; + } + + switch (sensor->id) { + case ST_LIS2DUXS12_ID_FSM_0: + case ST_LIS2DUXS12_ID_FSM_1: + case ST_LIS2DUXS12_ID_FSM_2: + case ST_LIS2DUXS12_ID_FSM_3: + case ST_LIS2DUXS12_ID_FSM_4: + case ST_LIS2DUXS12_ID_FSM_5: + case ST_LIS2DUXS12_ID_FSM_6: + case ST_LIS2DUXS12_ID_FSM_7: + case ST_LIS2DUXS12_ID_MLC_0: + case ST_LIS2DUXS12_ID_MLC_1: + case ST_LIS2DUXS12_ID_MLC_2: + case ST_LIS2DUXS12_ID_MLC_3: + if ((hw->settings->st_qvar_support) && + (hw->mlc_config->requested_device & + BIT(ST_LIS2DUXS12_ID_QVAR)) && + !(hw->enable_mask & BIT(ST_LIS2DUXS12_ID_QVAR))) { + err = st_lis2duxs12_write_with_mask_locked(hw, + ST_LIS2DUXS12_AH_QVAR_CFG_ADDR, + ST_LIS2DUXS12_AH_QVAR_EN_MASK, + req_odr > 0 ? 1 : 0); + if (err < 0) + return err; + } + break; + default: + break; + } + + if (ST_LIS2DUXS12_ODR_EXPAND(req_odr, req_uodr) > 0) { + err = st_lis2duxs12_get_odr_val(id, req_odr, req_uodr, + &oe); + if (err) + return err; + + /* check if sensor supports power mode setting */ + if (sensor->pm != ST_LIS2DUXS12_NO_MODE) { + err = st_lis2duxs12_update_bits_locked(hw, + st_lis2duxs12_odr_table[id].pm.addr, + st_lis2duxs12_odr_table[id].pm.mask, + sensor->pm); + if (err < 0) + return err; + } + } + + return st_lis2duxs12_update_bits_locked(hw, + st_lis2duxs12_odr_table[id].reg.addr, + st_lis2duxs12_odr_table[id].reg.mask, + oe.val); +} + +int st_lis2duxs12_sensor_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_lis2duxs12_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; +} + +static int +st_lis2duxs12_read_oneshot(struct st_lis2duxs12_sensor *sensor, + u8 addr, int *val) +{ + struct st_lis2duxs12_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + if (sensor->id == ST_LIS2DUXS12_ID_TEMP) { + u8 status; + + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_STATUS_ADDR, + &status, sizeof(status)); + if (err < 0) + return err; + + if (status & ST_LIS2DUXS12_DRDY_MASK) { + err = st_lis2duxs12_read_locked(hw, addr, + &data, + sizeof(data)); + if (err < 0) + return err; + + sensor->old_data = data; + } else { + data = sensor->old_data; + } + } else { + err = st_lis2duxs12_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + /* + * use big delay for data valid because of drdy mask + * enabled uodr is neglected in this operation + */ + delay = 10000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lis2duxs12_read_locked(hw, addr, &data, + sizeof(data)); + + st_lis2duxs12_sensor_set_enable(sensor, false); + if (err < 0) + return err; + } + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lis2duxs12_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lis2duxs12_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_lis2duxs12_read_oneshot(sensor, ch->address, + val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + 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 = 1; + *val2 = ST_LIS2DUXS12_TEMP_GAIN; + ret = IIO_VAL_FRACTIONAL; + break; + case IIO_ACCEL: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + return -EINVAL; + } + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lis2duxs12_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lis2duxs12_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + struct st_lis2duxs12_odr oe = { 0 }; + + err = st_lis2duxs12_get_odr_val(sensor->id, + val, val2, &oe); + if (!err) { + if (sensor->hw->enable_mask & BIT(sensor->id)) { + switch (sensor->id) { + case ST_LIS2DUXS12_ID_ACC: { + err = st_lis2duxs12_set_odr(sensor, + oe.hz, + oe.uhz); + if (err < 0) + break; + + st_lis2duxs12_set_std_level(sensor->hw, + oe.hz); + } + break; + default: + break; + } + } + + sensor->odr = oe.hz; + sensor->uodr = oe.uhz; + } + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static int +st_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +static int +st_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_lis2duxs12_embfunc_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_lis2duxs12_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lis2duxs12_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lis2duxs12_odr_table[id].size; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lis2duxs12_odr_table[id].odr_avl[i].hz, + st_lis2duxs12_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lis2duxs12_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lis2duxs12_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lis2duxs12_fs_table[id].size; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + st_lis2duxs12_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lis2duxs12_sysfs_get_power_mode_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_power_mode); i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%s ", + st_lis2duxs12_power_mode[i].string_mode); + } + + buf[len - 1] = '\n'; + + return len; +} + +ssize_t st_lis2duxs12_get_power_mode(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%s\n", + st_lis2duxs12_power_mode[sensor->pm].string_mode); +} + +ssize_t st_lis2duxs12_set_power_mode(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + int err, i; + + for (i = 0; i < ARRAY_SIZE(st_lis2duxs12_power_mode); i++) { + if (strncmp(buf, st_lis2duxs12_power_mode[i].string_mode, + strlen(st_lis2duxs12_power_mode[i].string_mode)) == 0) + break; + } + + if (i == ARRAY_SIZE(st_lis2duxs12_power_mode)) + return -EINVAL; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + /* update power mode */ + sensor->pm = st_lis2duxs12_power_mode[i].val; + + iio_device_release_direct_mode(iio_dev); + + return size; +} + +int st_lis2duxs12_get_int_reg(struct st_lis2duxs12_hw *hw) +{ + int err, ft_int_pin, md_int_pin, emb_int_pin; + struct device_node *np = hw->dev->of_node; + + if (!np) + return -EINVAL; + + err = of_property_read_u32(np, "st,int-pin", &ft_int_pin); + if (err < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + ft_int_pin = pdata ? pdata->drdy_int_pin : 1; + } + + err = of_property_read_u32(np, "st,md-int-pin", &md_int_pin); + if (err < 0) + md_int_pin = ft_int_pin; + + err = of_property_read_u32(np, "st,emb-int-pin", &emb_int_pin); + if (err < 0) + emb_int_pin = ft_int_pin; + + switch (ft_int_pin) { + case 1: + hw->ft_int_reg = ST_LIS2DUXS12_CTRL2_ADDR; + break; + case 2: + hw->ft_int_reg = ST_LIS2DUXS12_CTRL3_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + switch (md_int_pin) { + case 1: + hw->md_int_reg = ST_LIS2DUXS12_MD1_CFG_ADDR; + break; + case 2: + hw->md_int_reg = ST_LIS2DUXS12_MD2_CFG_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + switch (emb_int_pin) { + case 1: + hw->emb_int_reg = ST_LIS2DUXS12_EMB_FUNC_INT1_ADDR; + break; + case 2: + hw->emb_int_reg = ST_LIS2DUXS12_EMB_FUNC_INT2_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + hw->int_pin = ft_int_pin; + + return 0; +} + +static ssize_t +st_lis2duxs12_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lis2duxs12_selftest_table[1].string_mode, + st_lis2duxs12_selftest_table[2].string_mode); +} + +static ssize_t +st_lis2duxs12_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int8_t result; + char *message; + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lis2duxs12_sensor_id id = sensor->id; + + if (id != ST_LIS2DUXS12_ID_ACC) + 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_lis2duxs12_selftest_sensor(struct st_lis2duxs12_sensor *sensor, + int mode) +{ + enum st_lis2duxs12_pm_t pm; + int xyz[2][3]; + u8 raw_data[6]; + int i, ret; + int uodr; + int odr; + + pm = sensor->pm; + sensor->pm = ST_LIS2DUXS12_HP_MODE; + + ret = st_lis2duxs12_sensor_set_enable(sensor, false); + if (ret < 0) + return ret; + + /* wait 25 ms for stable output */ + msleep(25); + + if (mode == 1) { + ret = st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_CTRL3_ADDR, + GENMASK(1, 0), 0x03); + if (ret < 0) + goto selftest_stop; + + ret = st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_WAKE_UP_DUR_ADDR, + BIT(4), 1); + if (ret < 0) + goto selftest_stop; + } else { + ret = st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_CTRL3_ADDR, + GENMASK(1, 0), 0x00); + if (ret < 0) + goto selftest_stop; + + ret = st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_WAKE_UP_DUR_ADDR, + BIT(4), 0); + if (ret < 0) + goto selftest_stop; + } + + ret = st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_SELF_TEST_ADDR, + ST_LIS2DUXS12_ST_MASK, 0x02); + if (ret < 0) + goto selftest_stop; + + odr = sensor->odr; + uodr = sensor->uodr; + sensor->odr = 200; + sensor->uodr = 0; + + ret = st_lis2duxs12_sensor_set_enable(sensor, true); + if (ret < 0) + goto selftest_stop; + + /* wait 30 ms for stable output */ + msleep(30); + + ret = st_lis2duxs12_read_locked(sensor->hw, + ST_LIS2DUXS12_OUT_X_L_ADDR, + raw_data, sizeof(raw_data)); + if (ret < 0) + goto selftest_stop; + + for (i = 0; i < 3; i++) + xyz[0][i] = ((s16)*(u16 *)&raw_data[2 * i]); + + ret = st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_SELF_TEST_ADDR, + ST_LIS2DUXS12_ST_MASK, 0x01); + if (ret < 0) + goto selftest_stop; + + /* wait 30 ms for stable output */ + msleep(30); + + ret = st_lis2duxs12_read_locked(sensor->hw, + ST_LIS2DUXS12_OUT_X_L_ADDR, + raw_data, sizeof(raw_data)); + if (ret < 0) + goto selftest_stop; + + for (i = 0; i < 3; i++) { + xyz[1][i] = ((s16)*(u16 *)&raw_data[2 * i]); + if ((abs(xyz[1][i] - xyz[0][i]) < sensor->min_st) || + (abs(xyz[1][i] - xyz[0][i]) > sensor->max_st)) { + sensor->selftest_status = -1; + goto selftest_stop; + } + } + + sensor->selftest_status = 1; + +selftest_stop: + /* restore sensor configuration */ + st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_SELF_TEST_ADDR, + ST_LIS2DUXS12_ST_MASK, 0x00); + st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_CTRL3_ADDR, + GENMASK(1, 0), 0x00); + st_lis2duxs12_update_bits_locked(sensor->hw, + ST_LIS2DUXS12_WAKE_UP_DUR_ADDR, + BIT(4), 0); + sensor->pm = pm; + sensor->odr = odr; + sensor->uodr = uodr; + + return st_lis2duxs12_sensor_set_enable(sensor, false); +} + +static ssize_t +st_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + int ret, mode; + u32 gain; + + if (sensor->id != ST_LIS2DUXS12_ID_ACC) + return -EINVAL; + + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + return ret; + + /* self test mode unavailable if some sensors enabled */ + if (hw->enable_mask & + GENMASK(ST_LIS2DUXS12_ID_MAX, ST_LIS2DUXS12_ID_ACC)) { + ret = -EBUSY; + + goto out_claim; + } + + for (mode = 0; mode < ARRAY_SIZE(st_lis2duxs12_selftest_table); + mode++) { + if (strncmp(buf, st_lis2duxs12_selftest_table[mode].string_mode, + strlen(st_lis2duxs12_selftest_table[mode].string_mode)) == 0) + break; + } + + if (mode == ARRAY_SIZE(st_lis2duxs12_selftest_table)) + return -EINVAL; + + /* set BDU = 1, FS = 8g, BW = ODR/16, ODR = 200 Hz */ + gain = sensor->gain; + st_lis2duxs12_set_full_scale(sensor, IIO_G_TO_M_S_2(244)); + st_lis2duxs12_selftest_sensor(sensor, mode); + + /* restore full scale after test */ + st_lis2duxs12_set_full_scale(sensor, gain); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +static ssize_t +st_lis2duxs12_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 = st_lis2duxs12_reset_step_counter(iio_dev); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lis2duxs12_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lis2duxs12_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_lis2duxs12_sysfs_scale_avail, NULL, 0); + +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lis2duxs12_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lis2duxs12_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lis2duxs12_get_watermark, + st_lis2duxs12_set_watermark, 0); + +static IIO_DEVICE_ATTR(power_mode_available, 0444, + st_lis2duxs12_sysfs_get_power_mode_avail, NULL, 0); +static IIO_DEVICE_ATTR(power_mode, 0644, + st_lis2duxs12_get_power_mode, + st_lis2duxs12_set_power_mode, 0); + +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lis2duxs12_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_lis2duxs12_sysfs_get_selftest_status, + st_lis2duxs12_sysfs_start_selftest, 0); +static IIO_DEVICE_ATTR(reset_counter, 0200, NULL, + st_lis2duxs12_sysfs_reset_step_counter, 0); + +static struct attribute *st_lis2duxs12_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_lis2duxs12_acc_attribute_group = { + .attrs = st_lis2duxs12_acc_attributes, +}; + +static const struct iio_info st_lis2duxs12_acc_info = { + .attrs = &st_lis2duxs12_acc_attribute_group, + .read_raw = st_lis2duxs12_read_raw, + .write_raw = st_lis2duxs12_write_raw, +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_lis2duxs12_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static struct attribute *st_lis2duxs12_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, + NULL, +}; + +static const struct +attribute_group st_lis2duxs12_temp_attribute_group = { + .attrs = st_lis2duxs12_temp_attributes, +}; + +static const struct iio_info st_lis2duxs12_temp_info = { + .attrs = &st_lis2duxs12_temp_attribute_group, + .read_raw = st_lis2duxs12_read_raw, + .write_raw = st_lis2duxs12_write_raw, +}; + +static struct attribute *st_lis2duxs12_sc_attributes[] = { + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_reset_counter.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis2duxs12_sc_attribute_group = { + .attrs = st_lis2duxs12_sc_attributes, +}; + +static const struct iio_info st_lis2duxs12_sc_info = { + .attrs = &st_lis2duxs12_sc_attribute_group, +}; + +static struct attribute *st_lis2duxs12_sd_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2duxs12_sd_attribute_group = { + .attrs = st_lis2duxs12_sd_attributes, +}; + +static const struct iio_info st_lis2duxs12_sd_info = { + .attrs = &st_lis2duxs12_sd_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct attribute *st_lis2duxs12_sm_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2duxs12_sm_attribute_group = { + .attrs = st_lis2duxs12_sm_attributes, +}; + +static const struct iio_info st_lis2duxs12_sm_info = { + .attrs = &st_lis2duxs12_sm_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +static struct attribute *st_lis2duxs12_tilt_attributes[] = { + NULL, +}; + +static const struct +attribute_group st_lis2duxs12_tilt_attribute_group = { + .attrs = st_lis2duxs12_tilt_attributes, +}; + +static const struct iio_info st_lis2duxs12_tilt_info = { + .attrs = &st_lis2duxs12_tilt_attribute_group, + .read_event_config = st_lis2duxs12_read_event_config, + .write_event_config = st_lis2duxs12_write_event_config, +}; + +/* + * st_lis2duxs12_reset_device - sw reset + */ +static int st_lis2duxs12_reset_device(struct st_lis2duxs12_hw *hw) +{ + int ret; + + ret = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_CTRL1_ADDR, + ST_LIS2DUXS12_SW_RESET_MASK, + FIELD_PREP(ST_LIS2DUXS12_SW_RESET_MASK, 1)); + + if (ret < 0) + return ret; + + /* wait ~50 us */ + usleep_range(50, 51); + + ret = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_CTRL4_ADDR, + ST_LIS2DUXS12_BOOT_MASK, + FIELD_PREP(ST_LIS2DUXS12_BOOT_MASK, 1)); + + /* wait ~20 ms */ + usleep_range(20000, 20100); + + return ret; +} + +/* + * st_lis2duxs12_init_timestamp_engine - Init timestamp engine + */ +static int +st_lis2duxs12_init_timestamp_engine(struct st_lis2duxs12_hw *hw, + bool enable) +{ + int err; + + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_INTERRUPT_CFG_ADDR, + ST_LIS2DUXS12_TIMESTAMP_EN_MASK, + FIELD_PREP(ST_LIS2DUXS12_TIMESTAMP_EN_MASK, + enable)); + if (err < 0) + return err; + + hw->timestamp = enable; + + return err; +} + +static int st_lis2duxs12_init_device(struct st_lis2duxs12_hw *hw) +{ + int err; + + /* latch interrupts */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_INTERRUPT_CFG_ADDR, + ST_LIS2DUXS12_LIR_MASK, + FIELD_PREP(ST_LIS2DUXS12_LIR_MASK, 1)); + if (err < 0) + return err; + + /* enable Block Data Update */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_CTRL4_ADDR, + ST_LIS2DUXS12_BDU_MASK, + FIELD_PREP(ST_LIS2DUXS12_BDU_MASK, 1)); + if (err < 0) + return err; + + err = st_lis2duxs12_get_int_reg(hw); + if (err < 0) + return err; + + err = st_lis2duxs12_init_timestamp_engine(hw, true); + if (err < 0) + return err; + + + /* enable fifo configuration */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_CTRL4_ADDR, + ST_LIS2DUXS12_FIFO_EN_MASK, + FIELD_PREP(ST_LIS2DUXS12_FIFO_EN_MASK, 1)); + if (err < 0) + return err; + + /* enable XL and Temp */ + err = regmap_update_bits(hw->regmap, + ST_LIS2DUXS12_FIFO_WTM_ADDR, + ST_LIS2DUXS12_XL_ONLY_FIFO_MASK, + FIELD_PREP(ST_LIS2DUXS12_XL_ONLY_FIFO_MASK, 1)); + if (err < 0) + return err; + + hw->xl_only = true; + + /* enable FIFO watermak interrupt */ + return regmap_update_bits(hw->regmap, hw->ft_int_reg, + ST_LIS2DUXS12_INT_FIFO_TH_MASK, + FIELD_PREP(ST_LIS2DUXS12_INT_FIFO_TH_MASK, 1)); +} + +static struct +iio_dev *st_lis2duxs12_alloc_iiodev(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_sensor_id id) +{ + struct st_lis2duxs12_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; + + switch (id) { + case ST_LIS2DUXS12_ID_ACC: + iio_dev->channels = st_lis2duxs12_acc_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lis2duxs12_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_accel", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_acc_info; + iio_dev->available_scan_masks = + st_lis2duxs12_available_scan_masks; + sensor->max_watermark = ST_LIS2DUXS12_MAX_FIFO_DEPTH; + sensor->offset = 0; + sensor->pm = ST_LIS2DUXS12_HP_MODE; + sensor->odr = st_lis2duxs12_odr_table[id].odr_avl[3].hz; + sensor->uodr = + st_lis2duxs12_odr_table[id].odr_avl[3].uhz; + sensor->min_st = ST_LIS2DUXS12_SELFTEST_ACCEL_MIN; + sensor->max_st = ST_LIS2DUXS12_SELFTEST_ACCEL_MAX; + + /* set default FS to each sensor */ + sensor->gain = st_lis2duxs12_fs_table[id].fs_avl[0].gain; + break; + case ST_LIS2DUXS12_ID_TEMP: + iio_dev->channels = st_lis2duxs12_temp_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lis2duxs12_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_temp_info; + iio_dev->available_scan_masks = + st_lis2duxs12_temp_available_scan_masks; + sensor->max_watermark = ST_LIS2DUXS12_MAX_FIFO_DEPTH; + sensor->offset = ST_LIS2DUXS12_TEMP_OFFSET; + sensor->pm = ST_LIS2DUXS12_NO_MODE; + sensor->odr = st_lis2duxs12_odr_table[id].odr_avl[3].hz; + sensor->uodr = + st_lis2duxs12_odr_table[id].odr_avl[3].uhz; + + /* set default FS to each sensor */ + sensor->gain = st_lis2duxs12_fs_table[id].fs_avl[0].gain; + break; + case ST_LIS2DUXS12_ID_STEP_COUNTER: + iio_dev->channels = st_lis2duxs12_step_counter_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lis2duxs12_step_counter_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_step_c", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_sc_info; + iio_dev->available_scan_masks = + st_lis2duxs12_emb_available_scan_masks; + + /* request ODR @50 Hz to works properly */ + sensor->max_watermark = 1; + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_STEP_DETECTOR: + iio_dev->channels = st_lis2duxs12_step_detector_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lis2duxs12_step_detector_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_step_d", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_sd_info; + iio_dev->available_scan_masks = + st_lis2duxs12_emb_available_scan_masks; + + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_SIGN_MOTION: + iio_dev->channels = st_lis2duxs12_sign_motion_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lis2duxs12_sign_motion_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sign_motion", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_sm_info; + iio_dev->available_scan_masks = + st_lis2duxs12_emb_available_scan_masks; + + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + case ST_LIS2DUXS12_ID_TILT: + iio_dev->channels = st_lis2duxs12_tilt_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_tilt_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tilt", hw->settings->id.name); + iio_dev->info = &st_lis2duxs12_tilt_info; + iio_dev->available_scan_masks = + st_lis2duxs12_emb_available_scan_masks; + + /* request ODR @50 Hz to works properly */ + sensor->odr = 50; + sensor->uodr = 0; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static void st_lis2duxs12_disable_regulator_action(void *_data) +{ + struct st_lis2duxs12_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static int st_lis2duxs12_enable_regulator(struct st_lis2duxs12_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_lis2duxs12_disable_regulator_action, hw); + if (err) { + dev_err(hw->dev, + "Failed to setup regulator cleanup action %d\n", err); + return err; + } + + return err; +} + +int st_lis2duxs12_probe(struct device *dev, int irq, + enum st_lis2duxs12_hw_id hw_id, struct regmap *regmap) +{ + enum st_lis2duxs12_sensor_id id; + struct st_lis2duxs12_hw *hw; + int err; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->page_lock); + + hw->regmap = regmap; + hw->dev = dev; + hw->irq = irq; + hw->odr_table_entry = st_lis2duxs12_odr_table; + hw->fs_table_entry = st_lis2duxs12_fs_table; + + err = st_lis2duxs12_enable_regulator(hw); + if (err < 0) + return err; + + st_lis2duxs12_power_up_command(hw); + + err = st_lis2duxs12_check_whoami(hw, hw_id); + if (err < 0) + return err; + + err = st_lis2duxs12_reset_device(hw); + if (err < 0) + return err; + + err = st_lis2duxs12_init_device(hw); + if (err < 0) + return err; + +#if KERNEL_VERSION(5, 15, 0) <= LINUX_VERSION_CODE + err = iio_read_mount_matrix(hw->dev, &hw->orientation); +#elif KERNEL_VERSION(5, 2, 0) <= LINUX_VERSION_CODE + err = iio_read_mount_matrix(hw->dev, "mount-matrix", &hw->orientation); +#else /* LINUX_VERSION_CODE */ + err = of_iio_read_mount_matrix(hw->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 (id = ST_LIS2DUXS12_ID_ACC; + id <= ST_LIS2DUXS12_ID_TILT; id++) { + hw->iio_devs[id] = st_lis2duxs12_alloc_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + if (hw->settings->st_qvar_support) { + err = st_lis2duxs12_qvar_probe(hw); + if (err) + return err; + } + + err = st_lis2duxs12_probe_basicfunc(hw); + if (err < 0) + return err; + + err = st_lis2duxs12_mlc_probe(hw); + if (err < 0) + return err; + + err = st_lis2duxs12_embedded_function_init(hw); + if (err) + return err; + + if (hw->irq > 0) { + err = st_lis2duxs12_buffers_setup(hw); + if (err < 0) + return err; + } + + for (id = 0; id < ST_LIS2DUXS12_ID_MAX; id++) { + if (!hw->iio_devs[id]) + continue; + + err = devm_iio_device_register(hw->dev, + hw->iio_devs[id]); + if (err) + return err; + } + + err = st_lis2duxs12_mlc_init_preload(hw); + if (err) + return err; + +#if defined(CONFIG_PM) + device_init_wakeup(dev, 1); +#endif /* CONFIG_PM && CONFIG */ + + return 0; +} +EXPORT_SYMBOL(st_lis2duxs12_probe); + +int st_lis2duxs12_remove(struct device *dev) +{ + st_lis2duxs12_mlc_remove(dev); + + return 0; +} +EXPORT_SYMBOL(st_lis2duxs12_remove); + +static int __maybe_unused st_lis2duxs12_suspend(struct device *dev) +{ + struct st_lis2duxs12_hw *hw = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LIS2DUXS12_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + /* do not disable sensors if requested by wake-up */ + if (!((hw->enable_mask & BIT(sensor->id)) & + ST_LIS2DUXS12_WAKE_UP_SENSORS)) { + err = st_lis2duxs12_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } else { + err = st_lis2duxs12_set_odr(sensor, + ST_LIS2DUXS12_MIN_ODR_IN_WAKEUP, 0); + if (err < 0) + return err; + } + } + + if (st_lis2duxs12_is_fifo_enabled(hw)) { + err = st_lis2duxs12_suspend_fifo(hw); + if (err < 0) + return err; + } + + if (hw->enable_mask & ST_LIS2DUXS12_WAKE_UP_SENSORS) { + 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_lis2duxs12_resume(struct device *dev) +{ + struct st_lis2duxs12_hw *hw = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Resuming device\n"); + + if (hw->enable_mask & ST_LIS2DUXS12_WAKE_UP_SENSORS) { + if (device_may_wakeup(dev)) + disable_irq_wake(hw->irq); + } + + for (i = 0; i < ST_LIS2DUXS12_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_lis2duxs12_set_odr(sensor, sensor->odr, + sensor->uodr); + if (err < 0) + return err; + } + + if (st_lis2duxs12_is_fifo_enabled(hw)) + err = st_lis2duxs12_set_fifo_mode(hw, ST_LIS2DUXS12_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_lis2duxs12_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lis2duxs12_suspend, st_lis2duxs12_resume) +}; +EXPORT_SYMBOL(st_lis2duxs12_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2duxs12 driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2duxs12_embfunc.c b/drivers/iio/stm/accel/st_lis2duxs12_embfunc.c new file mode 100644 index 000000000000..b43a116ff95a --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_embfunc.c @@ -0,0 +1,205 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 embedded function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include + +#include "st_lis2duxs12.h" + +static int +st_lis2duxs12_ef_pg1_sensor_set_enable(struct st_lis2duxs12_sensor *sensor, + u8 mask, u8 irq_mask, bool enable) +{ + struct st_lis2duxs12_hw *hw = sensor->hw; + int err; + + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lis2duxs12_set_emb_access(hw, 1); + if (err < 0) + goto unlock; + + err = __st_lis2duxs12_write_with_mask(hw, + ST_LIS2DUXS12_EMB_FUNC_EN_A_ADDR, + mask, enable); + if (err < 0) + goto reset_page; + + err = __st_lis2duxs12_write_with_mask(hw, hw->emb_int_reg, + irq_mask, enable); + +reset_page: + st_lis2duxs12_set_emb_access(hw, 0); + + if (err < 0) + goto unlock; + + if (((hw->enable_mask & ST_LIS2DUXS12_EMB_FUNC_ENABLED) && enable) || + (!(hw->enable_mask & ST_LIS2DUXS12_EMB_FUNC_ENABLED) && !enable)) { + err = __st_lis2duxs12_write_with_mask(hw, hw->md_int_reg, + ST_LIS2DUXS12_INT_EMB_FUNC_MASK, + enable); + } + +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable Embedded Function sensor [EMB_FUN] + * + * @param sensor: ST ACC sensor instance + * @param enable: Enable/Disable sensor + * @return < 0 if error, 0 otherwise + */ +int st_lis2duxs12_embfunc_sensor_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable) +{ + int err; + + switch (sensor->id) { + case ST_LIS2DUXS12_ID_STEP_DETECTOR: + err = st_lis2duxs12_ef_pg1_sensor_set_enable(sensor, + ST_LIS2DUXS12_PEDO_EN_MASK, + ST_LIS2DUXS12_INT_STEP_DETECTOR_MASK, + enable); + break; + case ST_LIS2DUXS12_ID_SIGN_MOTION: + err = st_lis2duxs12_ef_pg1_sensor_set_enable(sensor, + ST_LIS2DUXS12_SIGN_MOTION_EN_MASK, + ST_LIS2DUXS12_INT_SIG_MOT_MASK, + enable); + break; + case ST_LIS2DUXS12_ID_TILT: + err = st_lis2duxs12_ef_pg1_sensor_set_enable(sensor, + ST_LIS2DUXS12_TILT_EN_MASK, + ST_LIS2DUXS12_INT_TILT_MASK, + enable); + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +int +st_lis2duxs12_step_counter_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable) +{ + struct st_lis2duxs12_hw *hw = sensor->hw; + bool run_enable = false; + int err; + + mutex_lock(&hw->page_lock); + err = st_lis2duxs12_set_emb_access(hw, 1); + if (err < 0) + goto unlock; + + err = __st_lis2duxs12_write_with_mask(hw, + ST_LIS2DUXS12_EMB_FUNC_EN_A_ADDR, + ST_LIS2DUXS12_PEDO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + err = __st_lis2duxs12_write_with_mask(hw, + ST_LIS2DUXS12_EMB_FUNC_FIFO_EN_ADDR, + ST_LIS2DUXS12_STEP_COUNTER_FIFO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + run_enable = true; + +reset_page: + st_lis2duxs12_set_emb_access(hw, 0); + +unlock: + mutex_unlock(&hw->page_lock); + + if (run_enable) + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + + return err; +} + +int st_lis2duxs12_reset_step_counter(struct iio_dev *iio_dev) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + __le16 data; + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_lis2duxs12_step_counter_set_enable(sensor, true); + if (err < 0) + goto unlock_iio_dev; + + mutex_lock(&hw->page_lock); + err = st_lis2duxs12_set_emb_access(hw, 1); + if (err < 0) + goto unlock_page; + + err = __st_lis2duxs12_write_with_mask(hw, + ST_LIS2DUXS12_EMB_FUNC_SRC_ADDR, + ST_LIS2DUXS12_PEDO_RST_STEP_MASK, 1); + if (err < 0) + goto reset_page; + + msleep(100); + + regmap_bulk_read(hw->regmap, ST_LIS2DUXS12_STEP_COUNTER_L_ADDR, + (u8 *)&data, sizeof(data)); + +reset_page: + st_lis2duxs12_set_emb_access(hw, 0); + +unlock_page: + mutex_unlock(&hw->page_lock); + + err = st_lis2duxs12_step_counter_set_enable(sensor, false); + +unlock_iio_dev: + iio_device_release_direct_mode(iio_dev); + + return err; +} + +int st_lis2duxs12_embedded_function_init(struct st_lis2duxs12_hw *hw) +{ + int err; + + err = st_lis2duxs12_update_bits_locked(hw, + ST_LIS2DUXS12_CTRL4_ADDR, + ST_LIS2DUXS12_EMB_FUNC_EN_MASK, + 1); + if (err < 0) + return err; + + usleep_range(5000, 6000); + + /* enable latched interrupts */ + err = st_lis2duxs12_update_page_bits_locked(hw, + ST_LIS2DUXS12_PAGE_RW_ADDR, + ST_LIS2DUXS12_EMB_FUNC_LIR_MASK, + 1); + + return err; +} diff --git a/drivers/iio/stm/accel/st_lis2duxs12_i2c.c b/drivers/iio/stm/accel/st_lis2duxs12_i2c.c new file mode 100644 index 000000000000..96b8d04abf9f --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_i2c.c @@ -0,0 +1,80 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lis2duxs12.h" + +static const struct regmap_config st_lis2duxs12_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lis2duxs12_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + enum st_lis2duxs12_hw_id hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, &st_lis2duxs12_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_lis2duxs12_probe(&client->dev, client->irq, hw_id, regmap); +} + +static int st_lis2duxs12_i2c_remove(struct i2c_client *client) +{ + return st_lis2duxs12_remove(&client->dev); +} + +static const struct of_device_id st_lis2duxs12_i2c_of_match[] = { + { + .compatible = "st," ST_LIS2DUX12_DEV_NAME, + .data = (void *)ST_LIS2DUX12_ID, + }, + { + .compatible = "st," ST_LIS2DUXS12_DEV_NAME, + .data = (void *)ST_LIS2DUXS12_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lis2duxs12_i2c_of_match); + +static const struct i2c_device_id st_lis2duxs12_i2c_id_table[] = { + { ST_LIS2DUX12_DEV_NAME, ST_LIS2DUX12_ID }, + { ST_LIS2DUXS12_DEV_NAME, ST_LIS2DUXS12_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lis2duxs12_i2c_id_table); + +static struct i2c_driver st_lis2duxs12_driver = { + .driver = { + .name = "st_" ST_LIS2DUXS12_DEV_NAME "_i2c", + .pm = &st_lis2duxs12_pm_ops, + .of_match_table = of_match_ptr(st_lis2duxs12_i2c_of_match), + }, + .probe = st_lis2duxs12_i2c_probe, + .remove = st_lis2duxs12_i2c_remove, + .id_table = st_lis2duxs12_i2c_id_table, +}; +module_i2c_driver(st_lis2duxs12_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2duxs12 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2duxs12_i3c.c b/drivers/iio/stm/accel/st_lis2duxs12_i3c.c new file mode 100644 index 000000000000..8df824199f71 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_i3c.c @@ -0,0 +1,62 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 i3c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2duxs12.h" + +static const struct i3c_device_id st_lis2duxs12_i3c_ids[] = { + I3C_DEVICE(0x0104, ST_LIS2DUXS12_WHOAMI_VAL, NULL), + {}, +}; +MODULE_DEVICE_TABLE(i3c, st_lis2duxs12_i3c_ids); + +static int st_lis2duxs12_i3c_probe(struct i3c_device *i3cdev) +{ + struct regmap_config st_lis2duxs12_i3c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + }; + const struct i3c_device_id *id = i3c_device_match_id(i3cdev, + st_lis2duxs12_i3c_ids); + struct regmap *regmap; + + regmap = devm_regmap_init_i3c(i3cdev, + &st_lis2duxs12_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_lis2duxs12_probe(&i3cdev->dev, 0, + (uintptr_t)id->data, regmap); +} + +static struct i3c_driver st_lis2duxs12_driver = { + .driver = { + .name = "st_" ST_LIS2DUXS12_DEV_NAME "_i3c", + .pm = &st_lis2duxs12_pm_ops, + }, + .probe = st_lis2duxs12_i3c_probe, + .id_table = st_lis2duxs12_i3c_ids, +}; +module_i3c_driver(st_lis2duxs12_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2duxs12 i3c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2duxs12_mlc.c b/drivers/iio/stm/accel/st_lis2duxs12_mlc.c new file mode 100644 index 000000000000..1182994c19e1 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_mlc.c @@ -0,0 +1,843 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 machine learning core driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lis2duxs12.h" + +#define ST_LIS2DUXS12_MLC_LOADER_VERSION "0.4" + +/* number of machine learning core available on device hardware */ +#define ST_LIS2DUXS12_MLC_MAX_NUMBER 4 +#define ST_LIS2DUXS12_FSM_MAX_NUMBER 8 + +#define ST_LIS2DUXS12_LOADER_CMD_WAIT 0xff + +#ifdef CONFIG_IIO_LIS2DUXS12_MLC_BUILTIN_FIRMWARE +static const u8 st_lis2duxs12_mlc_fw[] = { + #include "st_lis2duxs12_mlc.fw" +}; +DECLARE_BUILTIN_FIRMWARE(LIS2DUXS12_MLC_FIRMWARE_NAME, + st_lis2duxs12_mlc_fw); +#else /* CONFIG_IIO_LIS2DUXS12_MLC_BUILTIN_FIRMWARE */ +#define LIS2DUXS12_MLC_FIRMWARE_NAME "st_lis2duxs12_mlc.bin" +#endif /* CONFIG_IIO_LIS2DUXS12_MLC_BUILTIN_FIRMWARE */ + +static const u8 mlcdata[] = { + /* lis2duxs12_mlc_fsm_hpd.ucf */ + 0x14, 0x00, 0x13, 0x10, 0xff, 0x05, 0x3f, 0x80, 0x04, 0x00, + 0x05, 0x00, 0x39, 0x4b, 0x1a, 0x01, 0x0a, 0x00, 0x0b, 0x01, + 0x0e, 0x00, 0x0f, 0x00, 0x17, 0x40, 0x02, 0x01, 0x08, 0x54, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x01, 0x09, 0x01, 0x09, 0x00, + 0x09, 0x02, 0x02, 0x21, 0x08, 0x00, 0x09, 0x24, 0x09, 0x08, + 0x09, 0x18, 0x09, 0x00, 0x09, 0x0f, 0x09, 0x00, 0x09, 0x01, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0xee, 0x09, 0x02, 0x09, 0x04, 0x09, 0x0f, 0x09, 0x66, + 0x09, 0x99, 0x09, 0x33, 0x09, 0xf1, 0x09, 0x44, 0x09, 0x77, + 0x09, 0x22, 0x09, 0x00, 0x04, 0x00, 0x05, 0x11, 0x17, 0x80, + 0x3f, 0x00, 0x1f, 0x01, 0x14, 0x92, 0x14, 0x00, 0x13, 0x10, + 0xff, 0x05, 0x3f, 0x80, 0x04, 0x00, 0x05, 0x00, 0x39, 0x4b, + 0x1a, 0x01, 0x0a, 0x00, 0x0b, 0x01, 0x0e, 0x00, 0x0f, 0x00, + 0x17, 0x40, 0x02, 0x01, 0x08, 0x54, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x01, 0x09, 0x01, 0x09, 0x00, 0x09, 0x02, 0x02, 0x21, + 0x08, 0x00, 0x09, 0x24, 0x09, 0x08, 0x09, 0x18, 0x09, 0x00, + 0x09, 0x0f, 0x09, 0x00, 0x09, 0x01, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0xee, 0x09, 0x02, + 0x09, 0x04, 0x09, 0x0f, 0x09, 0x66, 0x09, 0x99, 0x09, 0x33, + 0x09, 0xf1, 0x09, 0x44, 0x09, 0x77, 0x09, 0x22, 0x09, 0x00, + 0x04, 0x00, 0x05, 0x11, 0x17, 0x80, 0x3f, 0x00, 0x1f, 0x01, + 0x14, 0x92, 0x13, 0x10, 0xff, 0x05, 0x14, 0x00, 0x3f, 0x80, + 0x04, 0x00, 0x05, 0x00, 0x17, 0x40, 0x02, 0x01, 0x08, 0xb6, + 0x09, 0x00, 0x09, 0x3c, 0x09, 0xfe, 0x09, 0x00, 0x09, 0x18, + 0x09, 0x01, 0x09, 0x01, 0x09, 0x00, 0x09, 0x14, 0x09, 0x01, + 0x09, 0x0a, 0x02, 0x01, 0x08, 0xc8, 0x09, 0xdc, 0x09, 0x00, + 0x09, 0x1a, 0x09, 0x01, 0x09, 0x26, 0x09, 0x01, 0x02, 0x01, + 0x08, 0xdc, 0x09, 0x1c, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0xa2, 0x09, 0x1d, 0x09, 0xaf, 0x09, 0x21, 0x09, 0xa2, + 0x09, 0x1d, 0x09, 0x1d, 0x09, 0xbf, 0x09, 0x68, 0x09, 0x3a, + 0x09, 0x3f, 0x09, 0x00, 0x09, 0x03, 0x09, 0x2c, 0x09, 0x00, + 0x09, 0xfc, 0x09, 0x00, 0x09, 0x7c, 0x09, 0x1f, 0x09, 0x00, + 0x02, 0x11, 0x08, 0x1a, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x3f, 0x00, 0x3f, 0x80, + 0x17, 0x40, 0x02, 0x11, 0x08, 0x26, 0x09, 0x00, 0x09, 0x58, + 0x09, 0x40, 0x09, 0xe0, 0x3f, 0x80, 0x17, 0x00, 0x04, 0x00, + 0x05, 0x10, 0x02, 0x01, 0x3f, 0x00, 0x1f, 0x01, 0x3f, 0x80, + 0x3a, 0x41, 0x17, 0x80, 0x04, 0x00, 0x05, 0x11, 0x02, 0x01, + 0x3f, 0x00, 0x0c, 0x00, 0x0e, 0x00, 0x10, 0x10, 0x11, 0x00, + 0x12, 0x00, 0x13, 0x10, 0x14, 0x92, 0x15, 0x00, 0x16, 0x00, + 0x17, 0x00, 0x18, 0x00, 0x1c, 0x00, 0x1d, 0x00, 0x1e, 0x00, + 0x1f, 0x01, 0x20, 0x00, 0x31, 0x9a, 0x32, 0x00, 0x33, 0x00, + 0x3d, 0x00, 0x3f, 0x00, 0x47, 0x00, 0x6f, 0x00, 0x70, 0x00, + 0x71, 0x00, 0x72, 0x00, 0x73, 0x00, 0x74, 0x00, 0x75, 0x00, + 0x3f, 0x80, 0x17, 0x80, 0x04, 0x00, 0x05, 0x11, 0x02, 0x01, + 0x3f, 0x00, +}; + +static const struct firmware st_lis2duxs12_mlc_preload = { + .size = sizeof(mlcdata), + .data = mlcdata +}; + +static struct +iio_dev *st_lis2duxs12_mlc_alloc_iio_dev(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_sensor_id id); + +static const unsigned long st_lis2duxs12_mlc_available_scan_masks[] = { + BIT(1), 0x0 +}; + +static int +st_lis2duxs12_mlc_enable_sensor(struct st_lis2duxs12_sensor *sensor, + bool enable) +{ + struct st_lis2duxs12_hw *hw = sensor->hw; + int i, id, err = 0; + + /* enable acc sensor as trigger */ + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + if (sensor->status == ST_LIS2DUXS12_MLC_ENABLED) { + int value; + + value = enable ? hw->mlc_config->mlc_int_mask : 0; + err = st_lis2duxs12_write_page_locked(hw, + hw->mlc_config->mlc_int_addr, + &value, 1); + if (err < 0) + return err; + + /* + * enable mlc core + * only one mlc enable bit so not need to check if + * other running + */ + err = st_lis2duxs12_update_page_bits_locked(hw, + ST_LIS2DUXS12_EMB_FUNC_EN_B_ADDR, + ST_LIS2DUXS12_MLC_EN_MASK, + ST_LIS2DUXS12_SHIFT_VAL(enable, + ST_LIS2DUXS12_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_LIS2DUXS12_FSM_ENABLED) { + int value; + + value = enable ? hw->mlc_config->fsm_int_mask : 0; + err = st_lis2duxs12_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_LIS2DUXS12_FSM_MAX_NUMBER; i++) { + id = st_lis2duxs12_fsm_sensor_list[i]; + if (hw->enable_mask & BIT(id)) + break; + } + + /* check for any other fsm already enabled */ + if (enable || i == ST_LIS2DUXS12_FSM_MAX_NUMBER) { + err = st_lis2duxs12_update_page_bits_locked(hw, + ST_LIS2DUXS12_EMB_FUNC_EN_B_ADDR, + ST_LIS2DUXS12_FSM_EN_MASK, + ST_LIS2DUXS12_SHIFT_VAL(enable, + ST_LIS2DUXS12_FSM_EN_MASK)); + if (err < 0) + return err; + + /* force mlc enable */ + err = st_lis2duxs12_update_page_bits_locked(hw, + ST_LIS2DUXS12_EMB_FUNC_EN_B_ADDR, + ST_LIS2DUXS12_MLC_EN_MASK, + ST_LIS2DUXS12_SHIFT_VAL(enable, + ST_LIS2DUXS12_MLC_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_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + + return st_lis2duxs12_mlc_enable_sensor(sensor, state); +} + +static int +st_lis2duxs12_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_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +static int +st_lis2duxs12_get_mlc_odr_val(struct st_lis2duxs12_hw *hw, u8 val_odr) +{ + int i; + + for (i = 0; i < hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].size; i++) { + if (hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].val == val_odr) + return hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].hz; + } + + return -EINVAL; +} + +/* checks mlc/fsm program consistence with hw device id */ +static int st_lis2duxs12_check_valid_mlc(const struct firmware *fw, + struct st_lis2duxs12_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_LIS2DUXS12_FUNC_CFG_ACCESS_ADDR) && + (val & ST_LIS2DUXS12_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = true; + } else if ((reg == ST_LIS2DUXS12_FUNC_CFG_ACCESS_ADDR) && + (val & ST_LIS2DUXS12_EMB_FUNC_REG_ACCESS_MASK) == 0) { + stmc_page = false; + } else if (stmc_page) { + continue; + } else if (reg == ST_LIS2DUXS12_AH_QVAR_CFG_ADDR) { + if (!hw->settings->st_qvar_support) + return -EINVAL; + break; + } + } + + return 0; +} + +/* parse and program mlc fragments */ +static int st_lis2duxs12_program_mlc(const struct firmware *fw, + struct st_lis2duxs12_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; + + mutex_lock(&hw->page_lock); + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + if ((reg == ST_LIS2DUXS12_FUNC_CFG_ACCESS_ADDR) && + (val & ST_LIS2DUXS12_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = true; + } else if ((reg == ST_LIS2DUXS12_FUNC_CFG_ACCESS_ADDR) && + (val & ST_LIS2DUXS12_EMB_FUNC_REG_ACCESS_MASK) == 0) { + stmc_page = false; + } else if (reg == ST_LIS2DUXS12_LOADER_CMD_WAIT) { + msleep(val); + } else if (stmc_page) { + switch (reg) { + case ST_LIS2DUXS12_MLC_INT1_ADDR: + case ST_LIS2DUXS12_MLC_INT2_ADDR: + mlc_int |= val; + mlc_num++; + skip = 1; + break; + case ST_LIS2DUXS12_FSM_INT1_ADDR: + case ST_LIS2DUXS12_FSM_INT2_ADDR: + fsm_int |= val; + fsm_num++; + skip = 1; + break; + case ST_LIS2DUXS12_EMB_FUNC_EN_B_ADDR: + skip = 1; + break; + default: + break; + } + } else { + switch (reg) { + case ST_LIS2DUXS12_CTRL5_ADDR: + /* save requested odr and skip write to reg */ + req_odr = max_t(u8, req_odr, + (val >> __ffs(ST_LIS2DUXS12_ODR_MASK)) & 0x0f); + skip = 1; + break; + case ST_LIS2DUXS12_AH_QVAR_CFG_ADDR: + /* check qvar requirement */ + if (val & ST_LIS2DUXS12_AH_QVAR_EN_MASK) + hw->mlc_config->requested_device |= + BIT(ST_LIS2DUXS12_ID_QVAR); + + /* remove qvar enable flag */ + val &= ~ST_LIS2DUXS12_AH_QVAR_EN_MASK; + skip = 1; + break; + case ST_LIS2DUXS12_MD1_CFG_ADDR: + case ST_LIS2DUXS12_MD2_CFG_ADDR: + /* just write int on emb functions */ + val &= ST_LIS2DUXS12_INT_EMB_FUNC_MASK; + break; + default: + skip = 1; + break; + } + } + + if (!skip) { + ret = regmap_write(hw->regmap, reg, val); + if (ret) { + dev_err(hw->dev, "regmap_write fails\n"); + + mutex_unlock(&hw->page_lock); + + return ret; + } + } + + skip = 0; + + if (mlc_num >= ST_LIS2DUXS12_MLC_MAX_NUMBER || + fsm_num >= ST_LIS2DUXS12_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_LIS2DUXS12_MLC_INT1_ADDR : + ST_LIS2DUXS12_MLC_INT2_ADDR); + + hw->mlc_config->status |= ST_LIS2DUXS12_MLC_ENABLED; + hw->mlc_config->mlc_configured += mlc_num; + hw->mlc_config->requested_odr = st_lis2duxs12_get_mlc_odr_val(hw, req_odr); + } + + if (fsm_num) { + hw->mlc_config->fsm_int_mask = fsm_int; + hw->mlc_config->fsm_int_addr = (hw->int_pin == 1 ? + ST_LIS2DUXS12_FSM_INT1_ADDR : + ST_LIS2DUXS12_FSM_INT2_ADDR); + + hw->mlc_config->status |= ST_LIS2DUXS12_FSM_ENABLED; + hw->mlc_config->fsm_configured += fsm_num; + hw->mlc_config->requested_odr = st_lis2duxs12_get_mlc_odr_val(hw, req_odr); + } + + mutex_unlock(&hw->page_lock); + + return fsm_num + mlc_num; +} + +static void st_lis2duxs12_mlc_update(const struct firmware *fw, void *context) +{ + struct st_lis2duxs12_hw *hw = context; + enum st_lis2duxs12_sensor_id id; + int ret, i; + + if (!fw) { + dev_err(hw->dev, "could not get binary firmware\n"); + + return; + } + + ret = st_lis2duxs12_check_valid_mlc(fw, hw); + if (ret < 0) { + dev_err(hw->dev, "unsupported mlc version for hw_id %d\n", + hw->settings->id.hw_id); + + return; + } + + ret = st_lis2duxs12_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_LIS2DUXS12_MLC_MAX_NUMBER; i++) { + if (mlc_mask & BIT(i)) { + id = st_lis2duxs12_mlc_sensor_list[i]; + hw->iio_devs[id] = + st_lis2duxs12_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_LIS2DUXS12_FSM_MAX_NUMBER; i++) { + if (fsm_mask & BIT(i)) { + id = st_lis2duxs12_fsm_sensor_list[i]; + hw->iio_devs[id] = + st_lis2duxs12_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: + if (hw->preload_mlc) { + hw->preload_mlc = 0; + + return; + } + + release_firmware(fw); +} + +static int st_lis2duxs12_mlc_flush_all(struct st_lis2duxs12_hw *hw) +{ + struct st_lis2duxs12_sensor *sensor_mlc; + struct iio_dev *iio_dev; + int ret = 0, id; + + for (id = ST_LIS2DUXS12_ID_MLC_0; id < ST_LIS2DUXS12_ID_MAX; id++) { + iio_dev = hw->iio_devs[id]; + if (!iio_dev) + continue; + + sensor_mlc = iio_priv(iio_dev); + ret = st_lis2duxs12_mlc_enable_sensor(sensor_mlc, false); + if (ret < 0) + break; + + iio_device_unregister(iio_dev); + kfree(iio_dev->channels); + iio_device_free(iio_dev); + hw->iio_devs[id] = NULL; + } + + return ret; +} + +static ssize_t st_lis2duxs12_mlc_info(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lis2duxs12_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_lis2duxs12_mlc_get_version(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "mlc loader Version %s\n", + ST_LIS2DUXS12_MLC_LOADER_VERSION); +} + +static ssize_t st_lis2duxs12_mlc_flush(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lis2duxs12_hw *hw = sensor->hw; + int ret; + + ret = st_lis2duxs12_mlc_flush_all(hw); + memset(hw->mlc_config, 0, sizeof(*hw->mlc_config)); + + return ret < 0 ? ret : size; +} + +static ssize_t st_lis2duxs12_mlc_upload(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err; + + err = request_firmware_nowait(THIS_MODULE, true, + LIS2DUXS12_MLC_FIRMWARE_NAME, + dev, GFP_KERNEL, + sensor->hw, + st_lis2duxs12_mlc_update); + + return err < 0 ? err : size; +} + +static ssize_t st_lis2duxs12_mlc_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lis2duxs12_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%d\n", hw->mlc_config->requested_odr); +} + +static IIO_DEVICE_ATTR(mlc_info, 0444, st_lis2duxs12_mlc_info, NULL, 0); +static IIO_DEVICE_ATTR(mlc_flush, 0200, NULL, st_lis2duxs12_mlc_flush, 0); +static IIO_DEVICE_ATTR(mlc_version, 0444, st_lis2duxs12_mlc_get_version, + NULL, 0); +static IIO_DEVICE_ATTR(load_mlc, 0200, NULL, st_lis2duxs12_mlc_upload, 0); +static IIO_DEVICE_ATTR(mlc_odr, 0444, st_lis2duxs12_mlc_odr, NULL, 0); + +static struct attribute *st_lis2duxs12_mlc_event_attributes[] = { + &iio_dev_attr_mlc_info.dev_attr.attr, + &iio_dev_attr_mlc_version.dev_attr.attr, + &iio_dev_attr_load_mlc.dev_attr.attr, + &iio_dev_attr_mlc_flush.dev_attr.attr, + &iio_dev_attr_mlc_odr.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_lis2duxs12_mlc_event_attribute_group = { + .attrs = st_lis2duxs12_mlc_event_attributes, +}; + +static const struct iio_info st_lis2duxs12_mlc_event_info = { + .attrs = &st_lis2duxs12_mlc_event_attribute_group, + .read_event_config = st_lis2duxs12_mlc_read_event_config, + .write_event_config = st_lis2duxs12_mlc_write_event_config, +}; + +static ssize_t st_lis2duxs12_mlc_x_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return scnprintf(buf, PAGE_SIZE, "%d.%02d\n", + sensor->odr, sensor->uodr); +} + +static IIO_DEVICE_ATTR(mlc_x_odr, 0444, st_lis2duxs12_mlc_x_odr, NULL, 0); + +static struct attribute *st_lis2duxs12_mlc_x_event_attributes[] = { + &iio_dev_attr_mlc_x_odr.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_lis2duxs12_mlc_x_event_attribute_group = { + .attrs = st_lis2duxs12_mlc_x_event_attributes, +}; + +static const struct iio_info st_lis2duxs12_mlc_x_event_info = { + .attrs = &st_lis2duxs12_mlc_x_event_attribute_group, + .read_event_config = st_lis2duxs12_mlc_read_event_config, + .write_event_config = st_lis2duxs12_mlc_write_event_config, +}; + +static struct +iio_dev *st_lis2duxs12_mlc_alloc_iio_dev(struct st_lis2duxs12_hw *hw, + enum st_lis2duxs12_sensor_id id) +{ + struct st_lis2duxs12_sensor *sensor; + struct iio_chan_spec *channels; + struct iio_dev *iio_dev; + + /* devm management only for ST_LIS2DUXS12_ID_MLC */ + if (id == ST_LIS2DUXS12_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; + sensor->pm = ST_LIS2DUXS12_NO_MODE; + + switch (id) { + case ST_LIS2DUXS12_ID_MLC: { + const struct iio_chan_spec st_lis2duxs12_mlc_channels[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = devm_kzalloc(hw->dev, + sizeof(st_lis2duxs12_mlc_channels), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lis2duxs12_mlc_channels, + sizeof(st_lis2duxs12_mlc_channels)); + + iio_dev->available_scan_masks = + st_lis2duxs12_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_mlc_channels); + iio_dev->info = &st_lis2duxs12_mlc_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc", hw->settings->id.name); + break; + } + case ST_LIS2DUXS12_ID_MLC_0: + case ST_LIS2DUXS12_ID_MLC_1: + case ST_LIS2DUXS12_ID_MLC_2: + case ST_LIS2DUXS12_ID_MLC_3: { + const struct iio_chan_spec st_lis2duxs12_mlc_x_ch[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lis2duxs12_mlc_x_ch), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lis2duxs12_mlc_x_ch, + sizeof(st_lis2duxs12_mlc_x_ch)); + + iio_dev->available_scan_masks = + st_lis2duxs12_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_mlc_x_ch); + iio_dev->info = &st_lis2duxs12_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc_%d", hw->settings->id.name, + id - ST_LIS2DUXS12_ID_MLC_0); + sensor->outreg_addr = ST_LIS2DUXS12_MLC1_SRC_ADDR + id - + ST_LIS2DUXS12_ID_MLC_0; + sensor->status = ST_LIS2DUXS12_MLC_ENABLED; + sensor->odr = hw->mlc_config->requested_odr; + sensor->uodr = 0; + break; + } + case ST_LIS2DUXS12_ID_FSM_0: + case ST_LIS2DUXS12_ID_FSM_1: + case ST_LIS2DUXS12_ID_FSM_2: + case ST_LIS2DUXS12_ID_FSM_3: + case ST_LIS2DUXS12_ID_FSM_4: + case ST_LIS2DUXS12_ID_FSM_5: + case ST_LIS2DUXS12_ID_FSM_6: + case ST_LIS2DUXS12_ID_FSM_7: { + const struct iio_chan_spec st_lis2duxs12_fsm_x_ch[] = { + ST_LIS2DUXS12_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lis2duxs12_fsm_x_ch), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lis2duxs12_fsm_x_ch, + sizeof(st_lis2duxs12_fsm_x_ch)); + + iio_dev->available_scan_masks = + st_lis2duxs12_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_fsm_x_ch); + iio_dev->info = &st_lis2duxs12_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_fsm_%d", hw->settings->id.name, + id - ST_LIS2DUXS12_ID_FSM_0); + sensor->outreg_addr = ST_LIS2DUXS12_FSM_OUTS1_ADDR + + id - ST_LIS2DUXS12_ID_FSM_0; + sensor->status = ST_LIS2DUXS12_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_lis2duxs12_mlc_check_status(struct st_lis2duxs12_hw *hw) +{ + struct st_lis2duxs12_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_LIS2DUXS12_MLC_ENABLED) { + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_MLC_STATUS_MAINPAGE_ADDR, + (void *)&mlc_status, 1); + if (err) + return err; + + if (mlc_status) { + for (i = 0; i < ST_LIS2DUXS12_MLC_MAX_NUMBER; i++) { + id = st_lis2duxs12_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_lis2duxs12_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_LIS2DUXS12_FSM_ENABLED) { + err = st_lis2duxs12_read_locked(hw, + ST_LIS2DUXS12_FSM_STATUS_MAINPAGE_ADDR, + (void *)&fsm_status, 1); + if (err) + return err; + + if (fsm_status) { + for (i = 0; i < ST_LIS2DUXS12_FSM_MAX_NUMBER; i++) { + id = st_lis2duxs12_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_lis2duxs12_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, fsm_status, (u64)event[i]); + } + } + } + } + + return err; +} + +int st_lis2duxs12_mlc_init_preload(struct st_lis2duxs12_hw *hw) +{ + hw->preload_mlc = 1; + st_lis2duxs12_mlc_update(&st_lis2duxs12_mlc_preload, hw); + + return 0; +} + +int st_lis2duxs12_mlc_probe(struct st_lis2duxs12_hw *hw) +{ + hw->iio_devs[ST_LIS2DUXS12_ID_MLC] = + st_lis2duxs12_mlc_alloc_iio_dev(hw, ST_LIS2DUXS12_ID_MLC); + if (!hw->iio_devs[ST_LIS2DUXS12_ID_MLC]) + return -ENOMEM; + + hw->mlc_config = devm_kzalloc(hw->dev, + sizeof(struct st_lis2duxs12_mlc_config_t), + GFP_KERNEL); + if (!hw->mlc_config) + return -ENOMEM; + + return 0; +} + +int st_lis2duxs12_mlc_remove(struct device *dev) +{ + struct st_lis2duxs12_hw *hw = dev_get_drvdata(dev); + + return st_lis2duxs12_mlc_flush_all(hw); +} +EXPORT_SYMBOL(st_lis2duxs12_mlc_remove); diff --git a/drivers/iio/stm/accel/st_lis2duxs12_qvar.c b/drivers/iio/stm/accel/st_lis2duxs12_qvar.c new file mode 100644 index 000000000000..a0be73d47c5b --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_qvar.c @@ -0,0 +1,264 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 qvar sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2duxs12.h" + +#define ST_LIS2DUXS12_REG_OUT_T_AH_QVAR_L_ADDR 0x2e + +static const struct iio_chan_spec st_lis2duxs12_qvar_channels[] = { + ST_LIS2DUXS12_DATA_CHANNEL(IIO_ALTVOLTAGE, + ST_LIS2DUXS12_REG_OUT_T_AH_QVAR_L_ADDR, + 0, 0, 0, 12, 16, 's', + NULL), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static int st_lis2duxs12_qvar_init(struct st_lis2duxs12_hw *hw) +{ + /* impedance selection */ + return st_lis2duxs12_write_with_mask_locked(hw, + ST_LIS2DUXS12_AH_QVAR_CFG_ADDR, + ST_LIS2DUXS12_AH_QVAR_C_ZIN_MASK, 3); +} + +static ssize_t +st_lis2duxs12_sysfs_qvar_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2duxs12_hw *hw = sensor->hw; + int len = 0; + int i; + + /* qvar share the same XL odr table */ + for (i = 0; i < hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].size; i++) { + if (!hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].hz) + continue; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].hz, + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static int +st_lis2duxs12_get_qvar_odr_val(struct st_lis2duxs12_hw *hw, + int odr, int uodr, + struct st_lis2duxs12_odr *oe) +{ + int req_odr = ST_LIS2DUXS12_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].size; i++) { + sensor_odr = ST_LIS2DUXS12_ODR_EXPAND( + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].hz, + hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].uhz); + if (sensor_odr >= req_odr) { + oe->hz = hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].hz; + oe->uhz = hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].uhz; + oe->val = hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[i].val; + + return 0; + } + } + + return -EINVAL; +} + +static int +st_lis2duxs12_qvar_read_oneshot(struct st_lis2duxs12_sensor *sensor, + u8 addr, int *val) +{ + struct st_lis2duxs12_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + err = st_lis2duxs12_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + err = st_lis2duxs12_read_locked(hw, addr, &data, sizeof(data)); + + st_lis2duxs12_sensor_set_enable(sensor, false); + if (err < 0) + return err; + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lis2duxs12_qvar_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + *val = 1; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + return ret; + + ret = st_lis2duxs12_qvar_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; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int +st_lis2duxs12_qvar_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lis2duxs12_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + struct st_lis2duxs12_odr oe = { 0 }; + + err = st_lis2duxs12_get_qvar_odr_val(sensor->hw, + val, val2, &oe); + if (!err) { + sensor->odr = oe.hz; + sensor->uodr = oe.uhz; + } + break; + } + default: + err = -EINVAL; + break; + } + + iio_device_release_direct_mode(iio_dev); + + return err; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lis2duxs12_sysfs_qvar_sampling_freq_avail); + +static struct attribute *st_lis2duxs12_qvar_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis2duxs12_qvar_attribute_group = { + .attrs = st_lis2duxs12_qvar_attributes, +}; + +static const struct iio_info st_lis2duxs12_qvar_info = { + .attrs = &st_lis2duxs12_qvar_attribute_group, + .read_raw = st_lis2duxs12_qvar_read_raw, + .write_raw = st_lis2duxs12_qvar_write_raw, +}; + +static const unsigned long st_lis2duxs12_qvar_available_scan_masks[] = { + 0x1, 0x0 +}; + +int st_lis2duxs12_qvar_set_enable(struct st_lis2duxs12_sensor *sensor, + bool enable) +{ + int err; + + err = st_lis2duxs12_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + return st_lis2duxs12_write_with_mask_locked(sensor->hw, + ST_LIS2DUXS12_AH_QVAR_CFG_ADDR, + ST_LIS2DUXS12_AH_QVAR_EN_MASK, + enable ? 1 : 0); +} + +struct iio_dev * +st_lis2duxs12_alloc_qvar_iiodev(struct st_lis2duxs12_hw *hw) +{ + struct st_lis2duxs12_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_LIS2DUXS12_ID_QVAR; + sensor->hw = hw; + + iio_dev->channels = st_lis2duxs12_qvar_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2duxs12_qvar_channels); + iio_dev->name = "lis2duxs12_qvar"; + iio_dev->info = &st_lis2duxs12_qvar_info; + iio_dev->available_scan_masks = + st_lis2duxs12_qvar_available_scan_masks; + + sensor->odr = hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[0].hz; + sensor->uodr = hw->odr_table_entry[ST_LIS2DUXS12_ID_ACC].odr_avl[0].uhz; + sensor->gain = 1; + sensor->watermark = 1; + + return iio_dev; +} + +int st_lis2duxs12_qvar_probe(struct st_lis2duxs12_hw *hw) +{ + int err; + + hw->iio_devs[ST_LIS2DUXS12_ID_QVAR] = + st_lis2duxs12_alloc_qvar_iiodev(hw); + if (!hw->iio_devs[ST_LIS2DUXS12_ID_QVAR]) + return -ENOMEM; + + err = st_lis2duxs12_qvar_init(hw); + + return err < 0 ? err : 0; +} diff --git a/drivers/iio/stm/accel/st_lis2duxs12_spi.c b/drivers/iio/stm/accel/st_lis2duxs12_spi.c new file mode 100644 index 000000000000..c08bd20aee96 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2duxs12_spi.c @@ -0,0 +1,80 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis2duxs12 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lis2duxs12.h" + +static const struct regmap_config st_lis2duxs12_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lis2duxs12_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_lis2duxs12_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_lis2duxs12_probe(&spi->dev, spi->irq, hw_id, regmap); +} + +static int st_lis2duxs12_spi_remove(struct spi_device *spi) +{ + return st_lis2duxs12_remove(&spi->dev); +} + +static const struct of_device_id st_lis2duxs12_spi_of_match[] = { + { + .compatible = "st," ST_LIS2DUX12_DEV_NAME, + .data = (void *)ST_LIS2DUX12_ID, + }, + { + .compatible = "st," ST_LIS2DUXS12_DEV_NAME, + .data = (void *)ST_LIS2DUXS12_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lis2duxs12_spi_of_match); + +static const struct spi_device_id st_lis2duxs12_spi_id_table[] = { + { ST_LIS2DUX12_DEV_NAME, ST_LIS2DUX12_ID }, + { ST_LIS2DUXS12_DEV_NAME, ST_LIS2DUXS12_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lis2duxs12_spi_id_table); + +static struct spi_driver st_lis2duxs12_driver = { + .driver = { + .name = "st_" ST_LIS2DUXS12_DEV_NAME "_spi", + .pm = &st_lis2duxs12_pm_ops, + .of_match_table = of_match_ptr(st_lis2duxs12_spi_of_match), + }, + .probe = st_lis2duxs12_spi_probe, + .remove = st_lis2duxs12_spi_remove, + .id_table = st_lis2duxs12_spi_id_table, +}; +module_spi_driver(st_lis2duxs12_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2duxs12 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2dw12.h b/drivers/iio/stm/accel/st_lis2dw12.h new file mode 100644 index 000000000000..26d2f8cae275 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2dw12.h @@ -0,0 +1,103 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lis2dw12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_LIS2DW12_H +#define ST_LIS2DW12_H + +#include +#include +#include + +#define ST_LIS2DW12_DEV_NAME "lis2dw12" +#define ST_IIS2DLPC_DEV_NAME "iis2dlpc" +#define ST_AIS2IH_DEV_NAME "ais2ih" +#define ST_LIS2DW12_MAX_WATERMARK 31 +#define ST_LIS2DW12_DATA_SIZE 6 + +struct st_lis2dw12_transfer_function { + int (*read)(struct device *dev, u8 addr, int len, u8 *data); + int (*write)(struct device *dev, u8 addr, int len, u8 *data); +}; + +#define ST_LIS2DW12_RX_MAX_LENGTH 96 +#define ST_LIS2DW12_TX_MAX_LENGTH 8 + +struct st_lis2dw12_transfer_buffer { + u8 rx_buf[ST_LIS2DW12_RX_MAX_LENGTH]; + u8 tx_buf[ST_LIS2DW12_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +enum st_lis2dw12_fifo_mode { + ST_LIS2DW12_FIFO_BYPASS = 0x0, + ST_LIS2DW12_FIFO_CONTINUOUS = 0x6, +}; + +enum st_lis2dw12_selftest_status { + ST_LIS2DW12_ST_RESET, + ST_LIS2DW12_ST_PASS, + ST_LIS2DW12_ST_FAIL, +}; + +enum st_lis2dw12_sensor_id { + ST_LIS2DW12_ID_ACC, + ST_LIS2DW12_ID_TAP_TAP, + ST_LIS2DW12_ID_TAP, + ST_LIS2DW12_ID_WU, + ST_LIS2DW12_ID_MAX, +}; + +struct st_lis2dw12_sensor { + enum st_lis2dw12_sensor_id id; + struct st_lis2dw12_hw *hw; + + u16 gain; + u16 odr; +}; + +struct st_lis2dw12_hw { + struct device *dev; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + + struct iio_dev *iio_devs[ST_LIS2DW12_ID_MAX]; + + enum st_lis2dw12_selftest_status st_status; + u16 enable_mask; + + u8 watermark; + u8 std_level; + u64 samples; + + s64 delta_ts; + s64 ts_irq; + s64 ts; + + const struct st_lis2dw12_transfer_function *tf; + struct st_lis2dw12_transfer_buffer tb; +}; + +int st_lis2dw12_probe(struct device *dev, int irq, + const struct st_lis2dw12_transfer_function *tf_ops); +int st_lis2dw12_fifo_setup(struct st_lis2dw12_hw *hw); +int st_lis2dw12_update_fifo_watermark(struct st_lis2dw12_hw *hw, u8 watermark); +int st_lis2dw12_write_with_mask(struct st_lis2dw12_hw *hw, u8 addr, u8 mask, + u8 val); +ssize_t st_lis2dw12_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lis2dw12_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +int st_lis2dw12_sensor_set_enable(struct st_lis2dw12_sensor *sensor, + bool enable); + +#endif /* ST_LIS2DW12_H */ + diff --git a/drivers/iio/stm/accel/st_lis2dw12_buffer.c b/drivers/iio/stm/accel/st_lis2dw12_buffer.c new file mode 100644 index 000000000000..41f5c056b184 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2dw12_buffer.c @@ -0,0 +1,328 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2dw12 fifo driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2dw12.h" + +#define ST_LIS2DW12_STATUS_ADDR 0x27 +#define ST_LIS2DW12_STATUS_FF_MASK BIT(0) +#define ST_LIS2DW12_STATUS_TAP_TAP_MASK BIT(4) +#define ST_LIS2DW12_STATUS_TAP_MASK BIT(3) +#define ST_LIS2DW12_STATUS_WU_MASK BIT(6) +#define ST_LIS2DW12_STATUS_FTH_MASK BIT(7) +#define ST_LIS2DW12_FIFO_CTRL_ADDR 0x2e +#define ST_LIS2DW12_FIFOMODE_MASK GENMASK(7, 5) +#define ST_LIS2DW12_FTH_MASK GENMASK(4, 0) +#define ST_LIS2DW12_FIFO_SAMPLES_ADDR 0x2f +#define ST_LIS2DW12_FIFO_SAMPLES_FTH_MASK BIT(7) +#define ST_LIS2DW12_FIFO_SAMPLES_OVR_MASK BIT(6) +#define ST_LIS2DW12_WU_SRC_ADDR 0x38 +#define ST_LIS2DW12_TAP_SRC_ADDR 0x39 +#define ST_LIS2DW12_STAP_SRC_MASK BIT(5) +#define ST_LIS2DW12_DTAP_SRC_MASK BIT(4) +#define ST_LIS2DW12_TAP_EVT_MASK GENMASK(2, 0) +#define ST_LIS2DW12_FIFO_SAMPLES_DIFF_MASK GENMASK(5, 0) + +static inline s64 st_lis2dw12_get_timestamp(struct st_lis2dw12_hw *hw) +{ + return iio_get_time_ns(hw->iio_devs[ST_LIS2DW12_ID_ACC]); +} + +#define ST_LIS2DW12_EWMA_LEVEL 120 +#define ST_LIS2DW12_EWMA_DIV 128 +static inline s64 st_lis2dw12_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LIS2DW12_EWMA_DIV - weight) * diff, + ST_LIS2DW12_EWMA_DIV); + + return old + incr; +} + +static int st_lis2dw12_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + enum st_lis2dw12_fifo_mode mode; + int err; + + if (enable) { + hw->ts_irq = hw->ts = st_lis2dw12_get_timestamp(hw); + hw->delta_ts = div_s64(1000000000LL, sensor->odr) * + hw->watermark; + hw->samples = 0; + } + + mode = enable ? ST_LIS2DW12_FIFO_CONTINUOUS : ST_LIS2DW12_FIFO_BYPASS; + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_FIFO_CTRL_ADDR, + ST_LIS2DW12_FIFOMODE_MASK, mode); + if (err < 0) + return err; + + return st_lis2dw12_sensor_set_enable(sensor, enable); +} + +int st_lis2dw12_update_fifo_watermark(struct st_lis2dw12_hw *hw, u8 watermark) +{ + return st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_FIFO_CTRL_ADDR, + ST_LIS2DW12_FTH_MASK, watermark); +} + +ssize_t st_lis2dw12_set_hwfifo_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_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + int err, val; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock; + } + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto unlock; + + if (val < 1 || val > ST_LIS2DW12_MAX_WATERMARK) { + err = -EINVAL; + goto unlock; + } + + err = st_lis2dw12_update_fifo_watermark(hw, val); + if (err < 0) + goto unlock; + + hw->watermark = val; + +unlock: + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : size; +} + +static int st_lis2dw12_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lis2dw12_update_fifo(iio_dev, true); +} + +static int st_lis2dw12_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lis2dw12_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lis2dw12_acc_buffer_setup_ops = { + .preenable = st_lis2dw12_buffer_preenable, + .postdisable = st_lis2dw12_buffer_postdisable, +}; + +static int st_lis2dw12_read_fifo(struct st_lis2dw12_hw *hw) +{ + u8 iio_buff[ALIGN(ST_LIS2DW12_DATA_SIZE, sizeof(s64)) + sizeof(s64)]; + u8 buff[ST_LIS2DW12_RX_MAX_LENGTH], status, samples; + struct iio_dev *iio_dev = hw->iio_devs[ST_LIS2DW12_ID_ACC]; + struct iio_chan_spec const *ch = iio_dev->channels; + int i, err, word_len, fifo_len, read_len = 0; + s64 delta_ts; + + err = hw->tf->read(hw->dev, ST_LIS2DW12_FIFO_SAMPLES_ADDR, + sizeof(status), &status); + if (err < 0) + return err; + + samples = status & ST_LIS2DW12_FIFO_SAMPLES_DIFF_MASK; + delta_ts = div_s64(hw->delta_ts, hw->watermark); + fifo_len = samples * ST_LIS2DW12_DATA_SIZE; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buff)); + err = hw->tf->read(hw->dev, ch[0].address, word_len, buff); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_LIS2DW12_DATA_SIZE) { + if (unlikely(++hw->samples < hw->std_level)) { + hw->ts += delta_ts; + continue; + } + + hw->ts = min_t(s64, st_lis2dw12_get_timestamp(hw), + hw->ts); + memcpy(iio_buff, &buff[i], ST_LIS2DW12_DATA_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buff, + hw->ts); + hw->ts += delta_ts; + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lis2dw12_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_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + s64 code; + int err; + + mutex_lock(&hw->fifo_lock); + + err = st_lis2dw12_read_fifo(hw); + hw->ts_irq = st_lis2dw12_get_timestamp(hw); + + mutex_unlock(&hw->fifo_lock); + + code = IIO_UNMOD_EVENT_CODE(IIO_ACCEL, -1, + IIO_EV_TYPE_FIFO_FLUSH, + IIO_EV_DIR_EITHER); + iio_push_event(iio_dev, code, hw->ts_irq); + + return err < 0 ? err : size; +} + + +static irqreturn_t st_lis2dw12_handler_irq(int irq, void *private) +{ + struct st_lis2dw12_hw *hw = private; + s64 ts; + + ts = st_lis2dw12_get_timestamp(hw); + hw->delta_ts = st_lis2dw12_ewma(hw->delta_ts, ts - hw->ts_irq, + ST_LIS2DW12_EWMA_LEVEL); + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lis2dw12_handler_thread(int irq, void *private) +{ + struct st_lis2dw12_hw *hw = private; + u8 status; + s64 code; + int err; + + err = hw->tf->read(hw->dev, ST_LIS2DW12_STATUS_ADDR, sizeof(status), + &status); + if (err < 0) + return IRQ_HANDLED; + + if (status & ST_LIS2DW12_STATUS_FTH_MASK) { + mutex_lock(&hw->fifo_lock); + st_lis2dw12_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + } + + if (((status & ST_LIS2DW12_STATUS_TAP_MASK) && + (hw->enable_mask & BIT(ST_LIS2DW12_ID_TAP))) || + ((status & ST_LIS2DW12_STATUS_TAP_TAP_MASK) && + (hw->enable_mask & BIT(ST_LIS2DW12_ID_TAP_TAP)))) { + struct iio_dev *iio_dev; + enum iio_chan_type type; + u8 source; + + err = hw->tf->read(hw->dev, ST_LIS2DW12_TAP_SRC_ADDR, + sizeof(source), &source); + if (err < 0) + return IRQ_HANDLED; + + /* Consider can have Tap and Double Tap events contemporarely */ + if (source & ST_LIS2DW12_DTAP_SRC_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DW12_ID_TAP_TAP]; + type = IIO_TAP_TAP; + code = IIO_UNMOD_EVENT_CODE(type, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, code, + st_lis2dw12_get_timestamp(hw)); + } + + if (source & ST_LIS2DW12_STAP_SRC_MASK) { + iio_dev = hw->iio_devs[ST_LIS2DW12_ID_TAP]; + type = IIO_TAP; + code = IIO_UNMOD_EVENT_CODE(type, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, code, + st_lis2dw12_get_timestamp(hw)); + } + } + + if (status & ST_LIS2DW12_STATUS_WU_MASK) { + u8 wu_src; + + err = hw->tf->read(hw->dev, ST_LIS2DW12_WU_SRC_ADDR, + sizeof(wu_src), &wu_src); + if (err < 0) + return IRQ_HANDLED; + + code = IIO_UNMOD_EVENT_CODE(IIO_TAP_TAP, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(hw->iio_devs[ST_LIS2DW12_ID_WU], IIO_GESTURE, + st_lis2dw12_get_timestamp(hw)); + } + + return IRQ_HANDLED; +} + +int st_lis2dw12_fifo_setup(struct st_lis2dw12_hw *hw) +{ + struct iio_dev *iio_dev = hw->iio_devs[ST_LIS2DW12_ID_ACC]; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + int ret; + + ret = devm_request_threaded_irq(hw->dev, hw->irq, + st_lis2dw12_handler_irq, + st_lis2dw12_handler_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + "st_lis2dw12", hw); + if (ret) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return ret; + } + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + ret = devm_iio_kfifo_buffer_setup(hw->dev, iio_dev, + INDIO_BUFFER_SOFTWARE, + &st_lis2dw12_acc_buffer_setup_ops); + if (ret) + return ret; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(iio_dev, buffer); + iio_dev->setup_ops = &st_lis2dw12_acc_buffer_setup_ops; + iio_dev->modes |= INDIO_BUFFER_SOFTWARE; +#endif /* LINUX_VERSION_CODE */ + + return 0; +} + diff --git a/drivers/iio/stm/accel/st_lis2dw12_core.c b/drivers/iio/stm/accel/st_lis2dw12_core.c new file mode 100644 index 000000000000..b265246b07f3 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2dw12_core.c @@ -0,0 +1,1048 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2dw12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lis2dw12.h" + +#define ST_LIS2DW12_WHOAMI_ADDR 0x0f +#define ST_LIS2DW12_WHOAMI_VAL 0x44 + +#define ST_LIS2DW12_CTRL1_ADDR 0x20 +#define ST_LIS2DW12_ODR_MASK GENMASK(7, 4) +#define ST_LIS2DW12_MODE_MASK GENMASK(3, 2) +#define ST_LIS2DW12_LP_MODE_MASK GENMASK(1, 0) + +#define ST_LIS2DW12_CTRL2_ADDR 0x21 +#define ST_LIS2DW12_BDU_MASK BIT(3) +#define ST_LIS2DW12_RESET_MASK BIT(6) + +#define ST_LIS2DW12_CTRL3_ADDR 0x22 +#define ST_LIS2DW12_LIR_MASK BIT(4) +#define ST_LIS2DW12_ST_MASK GENMASK(7, 6) + +#define ST_LIS2DW12_CTRL4_INT1_CTRL_ADDR 0x23 +#define ST_LIS2DW12_DRDY_MASK BIT(0) +#define ST_LIS2DW12_FTH_INT_MASK BIT(1) +#define ST_LIS2DW12_TAP_INT1_MASK BIT(6) +#define ST_LIS2DW12_TAP_TAP_INT1_MASK BIT(3) +#define ST_LIS2DW12_FF_INT1_MASK BIT(4) +#define ST_LIS2DW12_WU_INT1_MASK BIT(5) + +#define ST_LIS2DW12_CTRL5_INT2_CTRL_ADDR 0x24 + +#define ST_LIS2DW12_CTRL6_ADDR 0x25 +#define ST_LIS2DW12_LN_MASK BIT(2) +#define ST_LIS2DW12_FS_MASK GENMASK(5, 4) +#define ST_LIS2DW12_BW_MASK GENMASK(7, 6) + +#define ST_LIS2DW12_OUT_X_L_ADDR 0x28 +#define ST_LIS2DW12_OUT_Y_L_ADDR 0x2a +#define ST_LIS2DW12_OUT_Z_L_ADDR 0x2c + +#define ST_LIS2DW12_TAP_THS_X_ADDR 0x30 +#define ST_LIS2DW12_TAP_THS_Y_ADDR 0x31 +#define ST_LIS2DW12_TAP_THS_Z_ADDR 0x32 + +#define ST_LIS2DW12_TAP_AXIS_MASK GENMASK(7, 5) +#define ST_LIS2DW12_TAP_THS_MAK GENMASK(4, 0) + +#define ST_LIS2DW12_INT_DUR_ADDR 0x33 + +#define ST_LIS2DW12_WAKE_UP_THS_ADDR 0x34 +#define ST_LIS2DW12_WAKE_UP_THS_MAK GENMASK(5, 0) +#define ST_LIS2DW12_SINGLE_DOUBLE_TAP_MAK BIT(7) + +#define ST_LIS2DW12_FREE_FALL_ADDR 0x36 +#define ST_LIS2DW12_FREE_FALL_THS_MASK GENMASK(2, 0) +#define ST_LIS2DW12_FREE_FALL_DUR_MASK GENMASK(7, 3) + +#define ST_LIS2DW12_ABS_INT_CFG_ADDR 0x3f +#define ST_LIS2DW12_ALL_INT_MASK BIT(5) +#define ST_LIS2DW12_INT2_ON_INT1_MASK BIT(6) +#define ST_LIS2DW12_DRDY_PULSED_MASK BIT(7) + +#define ST_LIS2DW12_FS_2G_GAIN IIO_G_TO_M_S_2(244) +#define ST_LIS2DW12_FS_4G_GAIN IIO_G_TO_M_S_2(488) +#define ST_LIS2DW12_FS_8G_GAIN IIO_G_TO_M_S_2(976) +#define ST_LIS2DW12_FS_16G_GAIN IIO_G_TO_M_S_2(1952) + +#define ST_LIS2DW12_SELFTEST_MIN 285 +#define ST_LIS2DW12_SELFTEST_MAX 6150 + +struct st_lis2dw12_std_entry { + u16 odr; + u8 val; +}; + +struct st_lis2dw12_std_entry st_lis2dw12_std_table[] = { + { 12, 12 }, + { 25, 18 }, + { 50, 24 }, + { 100, 24 }, + { 200, 32 }, + { 400, 48 }, + { 800, 64 }, + { 1600, 64 }, +}; + +struct st_lis2dw12_odr { + u16 hz; + u8 val; +}; + +static const struct st_lis2dw12_odr st_lis2dw12_odr_table[] = { + { 0, 0x0 }, /* power-down */ + { 12, 0x2 }, /* LP 12.5Hz */ + { 25, 0x3 }, /* LP 25Hz*/ + { 50, 0x4 }, /* LP 50Hz*/ + { 100, 0x5 }, /* LP 100Hz*/ + { 200, 0x6 }, /* LP 200Hz*/ + { 400, 0x7 }, /* HP 400Hz*/ + { 800, 0x8 }, /* HP 800Hz*/ + { 1600, 0x9 }, /* HP 1600Hz*/ +}; + +struct st_lis2dw12_fs { + u32 gain; + u8 val; +}; + +static const struct st_lis2dw12_fs st_lis2dw12_fs_table[] = { + { ST_LIS2DW12_FS_2G_GAIN, 0x0 }, + { ST_LIS2DW12_FS_4G_GAIN, 0x1 }, + { ST_LIS2DW12_FS_8G_GAIN, 0x2 }, + { ST_LIS2DW12_FS_16G_GAIN, 0x3 }, +}; + +struct st_lis2dw12_selftest_req { + char *mode; + u8 val; +}; + +struct st_lis2dw12_selftest_req st_lis2dw12_selftest_table[] = { + { "disabled", 0x0 }, + { "positive-sign", 0x1 }, + { "negative-sign", 0x2 }, +}; + +#define ST_LIS2DW12_ACC_CHAN(addr, ch2, idx) \ +{ \ + .type = IIO_ACCEL, \ + .address = addr, \ + .modified = 1, \ + .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 = idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 14, \ + .storagebits = 16, \ + .shift = 2, \ + .endianness = IIO_LE, \ + }, \ +} + +const struct iio_event_spec st_lis2dw12_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +const struct iio_event_spec st_lis2dw12_rthr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_LIS2DW12_EVENT_CHANNEL(chan_type, evt_spec) \ +{ \ + .type = chan_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = evt_spec, \ + .num_event_specs = 1, \ +} + +static const struct iio_chan_spec st_lis2dw12_acc_channels[] = { + ST_LIS2DW12_ACC_CHAN(ST_LIS2DW12_OUT_X_L_ADDR, IIO_MOD_X, 0), + ST_LIS2DW12_ACC_CHAN(ST_LIS2DW12_OUT_Y_L_ADDR, IIO_MOD_Y, 1), + ST_LIS2DW12_ACC_CHAN(ST_LIS2DW12_OUT_Z_L_ADDR, IIO_MOD_Z, 2), + ST_LIS2DW12_EVENT_CHANNEL(IIO_ACCEL, &st_lis2dw12_fifo_flush_event), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lis2dw12_tap_tap_channels[] = { + ST_LIS2DW12_EVENT_CHANNEL(IIO_TAP_TAP, &st_lis2dw12_rthr_event), +}; + +static const struct iio_chan_spec st_lis2dw12_tap_channels[] = { + ST_LIS2DW12_EVENT_CHANNEL(IIO_TAP, &st_lis2dw12_rthr_event), +}; + +static const struct iio_chan_spec st_lis2dw12_wu_channels[] = { + ST_LIS2DW12_EVENT_CHANNEL(IIO_GESTURE, &st_lis2dw12_rthr_event), +}; + +int st_lis2dw12_write_with_mask(struct st_lis2dw12_hw *hw, u8 addr, u8 mask, + u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + goto unlock; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); + if (err < 0) + dev_err(hw->dev, "failed to write %02x register\n", addr); + +unlock: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_lis2dw12_set_fs(struct st_lis2dw12_sensor *sensor, u16 gain) +{ + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lis2dw12_fs_table); i++) + if (st_lis2dw12_fs_table[i].gain == gain) + break; + + if (i == ARRAY_SIZE(st_lis2dw12_fs_table)) + return -EINVAL; + + err = st_lis2dw12_write_with_mask(sensor->hw, ST_LIS2DW12_CTRL6_ADDR, + ST_LIS2DW12_FS_MASK, + st_lis2dw12_fs_table[i].val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static inline int st_lis2dw12_get_odr_idx(u16 odr, u8 *idx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_lis2dw12_odr_table); i++) + if (st_lis2dw12_odr_table[i].hz == odr) + break; + + if (i == ARRAY_SIZE(st_lis2dw12_odr_table)) + return -EINVAL; + + *idx = i; + + return 0; +} + +static int st_lis2dw12_set_std_level(struct st_lis2dw12_hw *hw, u16 odr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_lis2dw12_std_table); i++) + if (st_lis2dw12_std_table[i].odr == odr) + break; + + if (i == ARRAY_SIZE(st_lis2dw12_std_table)) + return -EINVAL; + + hw->std_level = st_lis2dw12_std_table[i].val; + + return 0; +} + +static u16 st_lis2dw12_check_odr_dependency(struct st_lis2dw12_hw *hw, u16 odr, + enum st_lis2dw12_sensor_id ref_id) +{ + struct st_lis2dw12_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = odr > 0; + u16 ret; + + if (enable) { + if (hw->enable_mask & BIT(ref_id)) + ret = (ref->odr < odr) ? odr : ref->odr; + else + ret = odr; + } else { + ret = (hw->enable_mask & BIT(ref_id)) ? ref->odr : 0; + } + + return ret; +} + +static int st_lis2dw12_set_odr(struct st_lis2dw12_sensor *sensor, u16 req_odr) +{ + struct st_lis2dw12_hw *hw = sensor->hw; + u8 mode, val, i; + int err, odr; + + for (i = 0; i < ST_LIS2DW12_ID_MAX; i++) { + if (i == sensor->id) + continue; + + odr = st_lis2dw12_check_odr_dependency(hw, req_odr, i); + if (odr != req_odr) + /* devince already configured */ + return 0; + } + + err = st_lis2dw12_get_odr_idx(req_odr, &i); + if (err < 0) + return err; + + mode = req_odr > 200 ? 0x1 : 0x0; + val = (st_lis2dw12_odr_table[i].val << __ffs(ST_LIS2DW12_ODR_MASK)) | + (mode << __ffs(ST_LIS2DW12_MODE_MASK)) | 0x01; + + err = hw->tf->write(hw->dev, ST_LIS2DW12_CTRL1_ADDR, sizeof(val), + &val); + + return err < 0 ? err : 0; +} + +static int st_lis2dw12_check_whoami(struct st_lis2dw12_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_LIS2DW12_WHOAMI_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_LIS2DW12_WHOAMI_VAL) { + dev_err(hw->dev, "wrong whoami %02x vs %02x\n", + data, ST_LIS2DW12_WHOAMI_VAL); + return -ENODEV; + } + + return 0; +} + +static int st_lis2dw12_of_get_drdy_pin(struct st_lis2dw12_hw *hw, + int *drdy_pin) +{ + struct device_node *np = hw->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin); +} + +static int st_lis2dw12_get_drdy_pin(struct st_lis2dw12_hw *hw, u8 *drdy_reg) +{ + int err = 0, drdy_pin; + + if (st_lis2dw12_of_get_drdy_pin(hw, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + *drdy_reg = ST_LIS2DW12_CTRL4_INT1_CTRL_ADDR; + break; + case 2: + *drdy_reg = ST_LIS2DW12_CTRL5_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +static int st_lis2dw12_init_hw(struct st_lis2dw12_hw *hw) +{ + u8 drdy_reg; + int err; + + /* soft reset the device */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL2_ADDR, + ST_LIS2DW12_RESET_MASK, 1); + if (err < 0) + return err; + + /* enable BDU */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL2_ADDR, + ST_LIS2DW12_BDU_MASK, 1); + if (err < 0) + return err; + + /* enable all interrupts */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_ABS_INT_CFG_ADDR, + ST_LIS2DW12_ALL_INT_MASK, 1); + if (err < 0) + return err; + + /* configure fifo watermark */ + err = st_lis2dw12_update_fifo_watermark(hw, hw->watermark); + if (err < 0) + return err; + + /* configure default free fall event threshold */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_FREE_FALL_ADDR, + ST_LIS2DW12_FREE_FALL_THS_MASK, 1); + if (err < 0) + return err; + + /* configure default free fall event duration */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_FREE_FALL_ADDR, + ST_LIS2DW12_FREE_FALL_DUR_MASK, 1); + if (err < 0) + return err; + + /* enable tap event on all axes */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_TAP_THS_Z_ADDR, + ST_LIS2DW12_TAP_AXIS_MASK, 0x7); + if (err < 0) + return err; + + /* configure default threshold for Tap event recognition */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_TAP_THS_X_ADDR, + ST_LIS2DW12_TAP_THS_MAK, 9); + if (err < 0) + return err; + + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_TAP_THS_Y_ADDR, + ST_LIS2DW12_TAP_THS_MAK, 9); + if (err < 0) + return err; + + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_TAP_THS_Z_ADDR, + ST_LIS2DW12_TAP_THS_MAK, 9); + if (err < 0) + return err; + + /* low noise enabled by default */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL6_ADDR, + ST_LIS2DW12_LN_MASK, 1); + if (err < 0) + return err; + + /* BW = ODR/4 */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL6_ADDR, + ST_LIS2DW12_BW_MASK, 1); + if (err < 0) + return err; + + /* configure interrupt pin */ + err = st_lis2dw12_get_drdy_pin(hw, &drdy_reg); + if (err < 0) + return err; + + return st_lis2dw12_write_with_mask(hw, drdy_reg, + ST_LIS2DW12_FTH_INT_MASK, 1); +} + +static ssize_t +st_lis2dw12_sysfs_sampling_frequency_avl(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 1; i < ARRAY_SIZE(st_lis2dw12_odr_table); i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lis2dw12_odr_table[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lis2dw12_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_lis2dw12_fs_table); i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + st_lis2dw12_fs_table[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +int st_lis2dw12_sensor_set_enable(struct st_lis2dw12_sensor *sensor, + bool enable) +{ + struct st_lis2dw12_hw *hw = sensor->hw; + u16 val = enable ? sensor->odr : 0; + int err; + + err = st_lis2dw12_set_odr(sensor, val); + if (err < 0) + return err; + + if (enable) + hw->enable_mask |= BIT(sensor->id); + else + hw->enable_mask &= ~BIT(sensor->id); + + return 0; +} + +static int st_lis2dw12_read_oneshot(struct st_lis2dw12_sensor *sensor, + u8 addr, int *val) +{ + struct st_lis2dw12_hw *hw = sensor->hw; + int err, delay; + u8 data[2]; + + err = st_lis2dw12_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + /* sample to discard, 3 * odr us */ + delay = 3000000 / sensor->odr; + usleep_range(delay, delay + 1); + + err = hw->tf->read(hw->dev, addr, sizeof(data), data); + if (err < 0) + return err; + + st_lis2dw12_sensor_set_enable(sensor, false); + + *val = (s16)get_unaligned_le16(data); + + return IIO_VAL_INT; +} + +static int st_lis2dw12_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lis2dw12_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + ret = -EBUSY; + mutex_unlock(&iio_dev->mlock); + break; + } + ret = st_lis2dw12_read_oneshot(sensor, ch->address, val); + mutex_unlock(&iio_dev->mlock); + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lis2dw12_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lis2dw12_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lis2dw12_set_fs(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_lis2dw12_set_std_level(sensor->hw, val); + if (err < 0) + break; + + err = st_lis2dw12_get_odr_idx(val, &data); + if (!err) + sensor->odr = val; + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static int st_lis2dw12_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_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +static int st_lis2dw12_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_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + u8 data[2] = {}, drdy_val, drdy_mask; + int err; + + /* Read initial configuration data */ + err = hw->tf->read(hw->dev, ST_LIS2DW12_INT_DUR_ADDR, + sizeof(data), data); + if (err < 0) + return -EINVAL; + + switch (sensor->id) { + case ST_LIS2DW12_ID_WU: + drdy_mask = ST_LIS2DW12_WU_INT1_MASK; + drdy_val = state ? 1 : 0; + data[1] = state ? 0x02 : 0; + break; + case ST_LIS2DW12_ID_TAP_TAP: + drdy_mask = ST_LIS2DW12_TAP_TAP_INT1_MASK; + drdy_val = state ? 1 : 0; + if (state) { + data[0] |= 0x7f; + data[1] |= 0x80; + } else { + data[0] &= ~0x7f; + data[1] &= ~0x80; + } + break; + case ST_LIS2DW12_ID_TAP: + drdy_mask = ST_LIS2DW12_TAP_INT1_MASK; + drdy_val = state ? 1 : 0; + if (state) { + data[0] |= 6; + } else { + data[0] &= ~6; + } + break; + default: + return -EINVAL; + } + + err = hw->tf->write(hw->dev, ST_LIS2DW12_INT_DUR_ADDR, + sizeof(data), data); + if (err < 0) + return err; + + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL4_INT1_CTRL_ADDR, + drdy_mask, drdy_val); + if (err < 0) + return err; + + return st_lis2dw12_sensor_set_enable(sensor, state); +} + +static ssize_t st_lis2dw12_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + + return sprintf(buf, "%d\n", hw->watermark); +} + +static ssize_t +st_lis2dw12_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LIS2DW12_MAX_WATERMARK); +} + +static ssize_t st_lis2dw12_get_selftest_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%s, %s\n", st_lis2dw12_selftest_table[1].mode, + st_lis2dw12_selftest_table[2].mode); +} + +static ssize_t st_lis2dw12_get_selftest_status(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + char *ret; + + switch (hw->st_status) { + case ST_LIS2DW12_ST_PASS: + ret = "pass"; + break; + case ST_LIS2DW12_ST_FAIL: + ret = "fail"; + break; + default: + case ST_LIS2DW12_ST_RESET: + ret = "na"; + break; + } + + return sprintf(buf, "%s\n", ret); +} + +static ssize_t st_lis2dw12_enable_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_lis2dw12_sensor *sensor = iio_priv(iio_dev); + struct st_lis2dw12_hw *hw = sensor->hw; + s16 acc_st_x = 0, acc_st_y = 0, acc_st_z = 0; + s16 acc_x = 0, acc_y = 0, acc_z = 0; + u8 data[ST_LIS2DW12_DATA_SIZE], val; + int i, err, gain; + + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock; + } + + for (i = 0; i < ARRAY_SIZE(st_lis2dw12_selftest_table); i++) + if (!strncmp(buf, st_lis2dw12_selftest_table[i].mode, + size - 2)) + break; + + if (i == ARRAY_SIZE(st_lis2dw12_selftest_table)) { + err = -EINVAL; + goto unlock; + } + + hw->st_status = ST_LIS2DW12_ST_RESET; + val = st_lis2dw12_selftest_table[i].val; + gain = sensor->gain; + + /* fs = 2g, odr = 50Hz */ + err = st_lis2dw12_set_fs(sensor, ST_LIS2DW12_FS_2G_GAIN); + if (err < 0) + goto unlock; + + err = st_lis2dw12_set_odr(sensor, 50); + if (err < 0) + goto unlock; + + msleep(200); + + for (i = 0; i < 5; i++) { + err = hw->tf->read(hw->dev, ST_LIS2DW12_OUT_X_L_ADDR, + sizeof(data), data); + if (err < 0) + goto unlock; + + acc_x += ((s16)get_unaligned_le16(&data[0]) >> 2) / 5; + acc_y += ((s16)get_unaligned_le16(&data[2]) >> 2) / 5; + acc_z += ((s16)get_unaligned_le16(&data[4]) >> 2) / 5; + + msleep(10); + } + + /* enable self test */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL3_ADDR, + ST_LIS2DW12_ST_MASK, val); + if (err < 0) + goto unlock; + + msleep(200); + + for (i = 0; i < 5; i++) { + err = hw->tf->read(hw->dev, ST_LIS2DW12_OUT_X_L_ADDR, + sizeof(data), data); + if (err < 0) + goto unlock; + + acc_st_x += ((s16)get_unaligned_le16(&data[0]) >> 2) / 5; + acc_st_y += ((s16)get_unaligned_le16(&data[2]) >> 2) / 5; + acc_st_z += ((s16)get_unaligned_le16(&data[4]) >> 2) / 5; + + msleep(10); + } + + if (abs(acc_st_x - acc_x) >= ST_LIS2DW12_SELFTEST_MIN && + abs(acc_st_x - acc_x) <= ST_LIS2DW12_SELFTEST_MAX && + abs(acc_st_y - acc_y) >= ST_LIS2DW12_SELFTEST_MIN && + abs(acc_st_y - acc_y) >= ST_LIS2DW12_SELFTEST_MIN && + abs(acc_st_z - acc_z) >= ST_LIS2DW12_SELFTEST_MIN && + abs(acc_st_z - acc_z) >= ST_LIS2DW12_SELFTEST_MIN) + hw->st_status = ST_LIS2DW12_ST_PASS; + else + hw->st_status = ST_LIS2DW12_ST_FAIL; + + /* disable self test */ + err = st_lis2dw12_write_with_mask(hw, ST_LIS2DW12_CTRL3_ADDR, + ST_LIS2DW12_ST_MASK, 0); + if (err < 0) + goto unlock; + + err = st_lis2dw12_set_fs(sensor, gain); + if (err < 0) + goto unlock; + + err = st_lis2dw12_sensor_set_enable(sensor, false); + +unlock: + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lis2dw12_sysfs_sampling_frequency_avl); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lis2dw12_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_lis2dw12_get_hwfifo_watermark, + st_lis2dw12_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lis2dw12_get_max_hwfifo_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, + st_lis2dw12_flush_fifo, 0); +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lis2dw12_get_selftest_avail, NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, st_lis2dw12_get_selftest_status, + st_lis2dw12_enable_selftest, 0); + +static struct attribute *st_lis2dw12_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.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.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, + NULL, +}; + +static const struct attribute_group st_lis2dw12_acc_attribute_group = { + .attrs = st_lis2dw12_acc_attributes, +}; + +static const struct iio_info st_lis2dw12_acc_info = { + .attrs = &st_lis2dw12_acc_attribute_group, + .read_raw = st_lis2dw12_read_raw, + .write_raw = st_lis2dw12_write_raw, +}; + +static struct attribute *st_lis2dw12_wu_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2dw12_wu_attribute_group = { + .attrs = st_lis2dw12_wu_attributes, +}; + +static const struct iio_info st_lis2dw12_wu_info = { + .attrs = &st_lis2dw12_wu_attribute_group, + .read_event_config = st_lis2dw12_read_event_config, + .write_event_config = st_lis2dw12_write_event_config, +}; + +static struct attribute *st_lis2dw12_tap_tap_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2dw12_tap_tap_attribute_group = { + .attrs = st_lis2dw12_tap_tap_attributes, +}; + +static const struct iio_info st_lis2dw12_tap_tap_info = { + .attrs = &st_lis2dw12_tap_tap_attribute_group, + .read_event_config = st_lis2dw12_read_event_config, + .write_event_config = st_lis2dw12_write_event_config, +}; + +static struct attribute *st_lis2dw12_tap_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lis2dw12_tap_attribute_group = { + .attrs = st_lis2dw12_tap_attributes, +}; + +static const struct iio_info st_lis2dw12_tap_info = { + .attrs = &st_lis2dw12_tap_attribute_group, + .read_event_config = st_lis2dw12_read_event_config, + .write_event_config = st_lis2dw12_write_event_config, +}; + +static const unsigned long st_lis2dw12_avail_scan_masks[] = { 0x7, 0x0 }; +static const unsigned long st_lis2dw12_event_avail_scan_masks[] = { 0x1, 0x0 }; + +static struct iio_dev *st_lis2dw12_alloc_iiodev(struct st_lis2dw12_hw *hw, + enum st_lis2dw12_sensor_id id) +{ + struct st_lis2dw12_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; + + switch (id) { + case ST_LIS2DW12_ID_ACC: + iio_dev->channels = st_lis2dw12_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2dw12_acc_channels); + iio_dev->name = "lis2dw12_accel"; + iio_dev->info = &st_lis2dw12_acc_info; + iio_dev->available_scan_masks = st_lis2dw12_avail_scan_masks; + + sensor->odr = st_lis2dw12_odr_table[1].hz; + sensor->gain = st_lis2dw12_fs_table[0].gain; + break; + case ST_LIS2DW12_ID_WU: + iio_dev->channels = st_lis2dw12_wu_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2dw12_wu_channels); + iio_dev->name = "lis2dw12_wk"; + iio_dev->info = &st_lis2dw12_wu_info; + iio_dev->available_scan_masks = + st_lis2dw12_event_avail_scan_masks; + + sensor->odr = st_lis2dw12_odr_table[7].hz; + break; + case ST_LIS2DW12_ID_TAP_TAP: + iio_dev->channels = st_lis2dw12_tap_tap_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lis2dw12_tap_tap_channels); + iio_dev->name = "lis2dw12_tap_tap"; + iio_dev->info = &st_lis2dw12_tap_tap_info; + iio_dev->available_scan_masks = + st_lis2dw12_event_avail_scan_masks; + + sensor->odr = st_lis2dw12_odr_table[7].hz; + break; + case ST_LIS2DW12_ID_TAP: + iio_dev->channels = st_lis2dw12_tap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis2dw12_tap_channels); + iio_dev->name = "lis2dw12_tap"; + iio_dev->info = &st_lis2dw12_tap_info; + iio_dev->available_scan_masks = + st_lis2dw12_event_avail_scan_masks; + + sensor->odr = st_lis2dw12_odr_table[7].hz; + break; + default: + return NULL; + } + + return iio_dev; +} + +int st_lis2dw12_probe(struct device *dev, int irq, + const struct st_lis2dw12_transfer_function *tf_ops) +{ + struct st_lis2dw12_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->fifo_lock); + mutex_init(&hw->lock); + + hw->dev = dev; + hw->irq = irq; + hw->tf = tf_ops; + hw->watermark = 1; + + err = st_lis2dw12_check_whoami(hw); + if (err < 0) + return err; + + err = st_lis2dw12_init_hw(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LIS2DW12_ID_MAX; i++) { + hw->iio_devs[i] = st_lis2dw12_alloc_iiodev(hw, i); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + if (hw->irq > 0) { + err = st_lis2dw12_fifo_setup(hw); + if (err) + return err; + } + + for (i = 0; i < ST_LIS2DW12_ID_MAX; i++) { + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + return err; + } + + return 0; +} +EXPORT_SYMBOL(st_lis2dw12_probe); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2dw12 driver"); +MODULE_LICENSE("GPL v2"); + diff --git a/drivers/iio/stm/accel/st_lis2dw12_i2c.c b/drivers/iio/stm/accel/st_lis2dw12_i2c.c new file mode 100644 index 000000000000..43fd7ec41f51 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2dw12_i2c.c @@ -0,0 +1,103 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2dw12 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include + +#include "st_lis2dw12.h" + +static int st_lis2dw12_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_lis2dw12_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_lis2dw12_transfer_function st_lis2dw12_transfer_fn = { + .read = st_lis2dw12_i2c_read, + .write = st_lis2dw12_i2c_write, +}; + +static int st_lis2dw12_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_lis2dw12_probe(&client->dev, client->irq, + &st_lis2dw12_transfer_fn); +} + +static const struct of_device_id st_lis2dw12_i2c_of_match[] = { + { + .compatible = "st,lis2dw12", + .data = ST_LIS2DW12_DEV_NAME, + }, + { + .compatible = "st,iis2dlpc", + .data = ST_IIS2DLPC_DEV_NAME, + }, + { + .compatible = "st,ais2ih", + .data = ST_AIS2IH_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lis2dw12_i2c_of_match); + +static const struct i2c_device_id st_lis2dw12_i2c_id_table[] = { + { ST_LIS2DW12_DEV_NAME }, + { ST_AIS2IH_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lis2dw12_i2c_id_table); + +static struct i2c_driver st_lis2dw12_driver = { + .driver = { + .name = "st_lis2dw12_i2c", + .of_match_table = of_match_ptr(st_lis2dw12_i2c_of_match), + }, + .probe = st_lis2dw12_i2c_probe, + .id_table = st_lis2dw12_i2c_id_table, +}; +module_i2c_driver(st_lis2dw12_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2dw12 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2dw12_spi.c b/drivers/iio/stm/accel/st_lis2dw12_spi.c new file mode 100644 index 000000000000..77c5d286c502 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2dw12_spi.c @@ -0,0 +1,111 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2dw12 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include + +#include "st_lis2dw12.h" + +#define SENSORS_SPI_READ BIT(7) + +static int st_lis2dw12_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_lis2dw12_hw *hw = spi_get_drvdata(spi); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_lis2dw12_spi_write(struct device *dev, u8 addr, int len, + u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_lis2dw12_hw *hw = spi_get_drvdata(spi); + + if (len >= ST_LIS2DW12_TX_MAX_LENGTH) + return -ENOMEM; + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_lis2dw12_transfer_function st_lis2dw12_transfer_fn = { + .read = st_lis2dw12_spi_read, + .write = st_lis2dw12_spi_write, +}; + +static int st_lis2dw12_spi_probe(struct spi_device *spi) +{ + return st_lis2dw12_probe(&spi->dev, spi->irq, + &st_lis2dw12_transfer_fn); +} + +static const struct of_device_id st_lis2dw12_spi_of_match[] = { + { + .compatible = "st,lis2dw12", + .data = ST_LIS2DW12_DEV_NAME, + }, + { + .compatible = "st,iis2dlpc", + .data = ST_IIS2DLPC_DEV_NAME, + }, + { + .compatible = "st,ais2ih", + .data = ST_AIS2IH_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lis2dw12_spi_of_match); + +static const struct spi_device_id st_lis2dw12_spi_id_table[] = { + { ST_LIS2DW12_DEV_NAME }, + { ST_AIS2IH_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lis2dw12_spi_id_table); + +static struct spi_driver st_lis2dw12_driver = { + .driver = { + .name = "st_lis2dw12_spi", + .of_match_table = of_match_ptr(st_lis2dw12_spi_of_match), + }, + .probe = st_lis2dw12_spi_probe, + .id_table = st_lis2dw12_spi_id_table, +}; +module_spi_driver(st_lis2dw12_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis2dw12 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2hh12.h b/drivers/iio/stm/accel/st_lis2hh12.h new file mode 100644 index 000000000000..7ab3a08b4a5a --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2hh12.h @@ -0,0 +1,214 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lis2hh12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef __LIS2HH12_H +#define __LIS2HH12_H + +#include +#include +#include + +#define LIS2HH12_WHO_AM_I_ADDR 0x0f +#define LIS2HH12_WHO_AM_I_DEF 0x41 + +#define LIS2HH12_CTRL1_ADDR 0x20 +#define LIS2HH12_CTRL2_ADDR 0x21 +#define LIS2HH12_CTRL3_ADDR 0x22 +#define LIS2HH12_CTRL4_ADDR 0x23 +#define LIS2HH12_CTRL5_ADDR 0x24 +#define LIS2HH12_CTRL6_ADDR 0x25 +#define LIS2HH12_CTRL7_ADDR 0x26 + +#define LIS2HH12_STATUS_ADDR 0x27 +#define LIS2HH12_DATA_XYZ_RDY 0x08 + +#define LIS2HH12_FIFO_CTRL_ADDR 0x2E + +#define LIS2HH12_OUTX_L_ADDR 0x28 +#define LIS2HH12_OUTY_L_ADDR 0x2A +#define LIS2HH12_OUTZ_L_ADDR 0x2C + +#define LIS2HH12_FIFO_THS_ADDR LIS2HH12_FIFO_CTRL_ADDR +#define LIS2HH12_FIFO_THS_MASK 0x1f + +#define LIS2HH12_FIFO_MODE_ADDR LIS2HH12_FIFO_CTRL_ADDR +#define LIS2HH12_FIFO_MODE_MASK 0xe0 +#define LIS2HH12_FIFO_MODE_BYPASS 0x00 +#define LIS2HH12_FIFO_MODE_STREAM 0x02 + +#define LIS2HH12_FIFO_SRC_ADDR 0x2F +#define LIS2HH12_FIFO_STATUS_ADDR LIS2HH12_FIFO_SRC_ADDR +#define LIS2HH12_FIFO_FSS_MASK 0x1F +#define LIS2HH12_FIFO_SRC_FTH_MASK 0x80 + +#define LIS2HH12_ODR_ADDR LIS2HH12_CTRL1_ADDR +#define LIS2HH12_ODR_MASK 0x70 +#define LIS2HH12_ODR_POWER_DOWN_VAL 0x00 +#define LIS2HH12_ODR_10HZ_VAL 0x01 +#define LIS2HH12_ODR_50HZ_VAL 0x02 +#define LIS2HH12_ODR_100HZ_VAL 0x03 +#define LIS2HH12_ODR_200HZ_VAL 0x04 +#define LIS2HH12_ODR_400HZ_VAL 0x05 +#define LIS2HH12_ODR_800HZ_VAL 0x06 +#define LIS2HH12_ODR_LIST_NUM 7 + +#define LIS2HH12_FS_ADDR LIS2HH12_CTRL4_ADDR +#define LIS2HH12_FS_MASK 0x30 +#define LIS2HH12_FS_2G_VAL 0x00 +#define LIS2HH12_FS_4G_VAL 0x02 +#define LIS2HH12_FS_8G_VAL 0x03 +#define LIS2HH12_FS_LIST_NUM 3 + +#define LIS2HH12_FS_2G_GAIN IIO_G_TO_M_S_2(61) +#define LIS2HH12_FS_4G_GAIN IIO_G_TO_M_S_2(122) +#define LIS2HH12_FS_8G_GAIN IIO_G_TO_M_S_2(244) + +#define LIS2HH12_INT_CFG_ADDR LIS2HH12_CTRL3_ADDR +#define LIS2HH12_INT_DRDY_MASK 0x01 +#define LIS2HH12_INT_FTH_MASK 0x02 +#define LIS2HH12_INT_FOVR_MASK 0x04 + +#define LIS2HH12_FIFO_EN_ADDR LIS2HH12_CTRL3_ADDR +#define LIS2HH12_FIFO_EN_MASK 0x80 + +#define LIS2HH12_BDU_ADDR LIS2HH12_CTRL1_ADDR +#define LIS2HH12_BDU_MASK 0x08 +#define LIS2HH12_SOFT_RESET_ADDR LIS2HH12_CTRL5_ADDR +#define LIS2HH12_SOFT_RESET_MASK 0x40 +#define LIS2HH12_LIR_ADDR LIS2HH12_CTRL7_ADDR +#define LIS2HH12_LIR1_MASK 0x04 +#define LIS2HH12_LIR2_MASK 0x08 + +#define LIS2HH12_MAX_FIFO_LENGHT 32 +#define LIS2HH12_MAX_FIFO_THS (LIS2HH12_MAX_FIFO_LENGHT - 1) +#define LIS2HH12_FIFO_NUM_AXIS 3 +#define LIS2HH12_FIFO_BYTE_X_AXIS 2 +#define LIS2HH12_FIFO_BYTE_FOR_SAMPLE (LIS2HH12_FIFO_NUM_AXIS * \ + LIS2HH12_FIFO_BYTE_X_AXIS) +#define LIS2HH12_TIMESTAMP_SIZE 8 + +#define LIS2HH12_EN_BIT 0x01 +#define LIS2HH12_DIS_BIT 0x00 + +#define LIS2HH12_MAX_CHANNEL_SPEC 5 + +#define LIS2HH12_ACCEL 0 +#define LIS2HH12_SENSORS_NUMB 1 + +#define LIS2HH12_DEV_NAME "lis2hh12" +#define SET_BIT(a, b) {a |= (1 << b);} +#define RESET_BIT(a, b) {a &= ~(1 << b);} +#define CHECK_BIT(a, b) (a & (1 << b)) + +#define ST_LIS2HH12_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &lis2hh12_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_LIS2HH12_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + lis2hh12_sysfs_get_hwfifo_enabled,\ + lis2hh12_sysfs_set_hwfifo_enabled, 0); + +#define ST_LIS2HH12_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + lis2hh12_sysfs_get_hwfifo_watermark,\ + lis2hh12_sysfs_set_hwfifo_watermark, 0); + +#define ST_LIS2HH12_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + lis2hh12_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_LIS2HH12_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + lis2hh12_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_LIS2HH12_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + lis2hh12_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + STREAM, +}; + +#define LIS2HH12_TX_MAX_LENGTH 12 +#define LIS2HH12_RX_MAX_LENGTH 8193 + +struct lis2hh12_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[LIS2HH12_RX_MAX_LENGTH]; + u8 tx_buf[LIS2HH12_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct lis2hh12_data; + +struct lis2hh12_transfer_function { + int (*write)(struct lis2hh12_data *cdata, u8 reg_addr, int len, u8 *data); + int (*read)(struct lis2hh12_data *cdata, u8 reg_addr, int len, u8 *data); +}; + +struct lis2hh12_sensor_data { + struct lis2hh12_data *cdata; + const char *name; + s64 timestamp; + u8 enabled; + u32 odr; + u32 gain; + u8 sindex; + u8 sample_to_discard; +}; + +struct lis2hh12_data { + const char *name; + u8 drdy_int_pin; + u8 enabled_sensor; + u8 hwfifo_enabled; + u8 hwfifo_watermark; + u32 common_odr; + int irq; + s64 timestamp; + s64 sensor_deltatime; + s64 sensor_timestamp; + u8 *fifo_data; + u16 fifo_size; + struct device *dev; + struct iio_dev *iio_sensors_dev[LIS2HH12_SENSORS_NUMB]; + struct iio_trigger *iio_trig[LIS2HH12_SENSORS_NUMB]; + const struct lis2hh12_transfer_function *tf; + struct lis2hh12_transfer_buffer tb; +}; + +int lis2hh12_common_probe(struct lis2hh12_data *cdata, int irq); +#ifdef CONFIG_PM +int lis2hh12_common_suspend(struct lis2hh12_data *cdata); +int lis2hh12_common_resume(struct lis2hh12_data *cdata); +#endif +int lis2hh12_allocate_rings(struct lis2hh12_data *cdata); +int lis2hh12_allocate_triggers(struct lis2hh12_data *cdata, + const struct iio_trigger_ops *trigger_ops); +int lis2hh12_trig_set_state(struct iio_trigger *trig, bool state); +int lis2hh12_read_register(struct lis2hh12_data *cdata, u8 reg_addr, int data_len, + u8 *data); +int lis2hh12_update_drdy_irq(struct lis2hh12_sensor_data *sdata, bool state); +int lis2hh12_set_enable(struct lis2hh12_sensor_data *sdata, bool enable); +int lis2hh12_update_fifo_ths(struct lis2hh12_data *cdata, u8 fifo_len); +void lis2hh12_common_remove(struct lis2hh12_data *cdata, int irq); +void lis2hh12_read_fifo(struct lis2hh12_data *cdata, bool check_fifo_len); +void lis2hh12_deallocate_rings(struct lis2hh12_data *cdata); +void lis2hh12_deallocate_triggers(struct lis2hh12_data *cdata); +int lis2hh12_set_fifo_mode(struct lis2hh12_data *cdata, enum fifo_mode fm); +void lis2hh12_read_xyz(struct lis2hh12_data *cdata); + +#endif /* __LIS2HH12_H */ diff --git a/drivers/iio/stm/accel/st_lis2hh12_buffer.c b/drivers/iio/stm/accel/st_lis2hh12_buffer.c new file mode 100644 index 000000000000..3e45839db3a5 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2hh12_buffer.c @@ -0,0 +1,169 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2hh12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2hh12.h" + +#define LIS2HH12_ACCEL_BUFFER_SIZE \ + ALIGN(LIS2HH12_FIFO_BYTE_FOR_SAMPLE + LIS2HH12_TIMESTAMP_SIZE, \ + LIS2HH12_TIMESTAMP_SIZE) + +static void lis2hh12_push_fifo_data(struct lis2hh12_data *cdata, u16 fifo_ptr) +{ + size_t offset; + u8 buffer[LIS2HH12_ACCEL_BUFFER_SIZE], out_buf_index; + struct iio_dev *indio_dev = cdata->iio_sensors_dev[LIS2HH12_ACCEL]; + struct lis2hh12_sensor_data *sdata; + + out_buf_index = 0; + + /* Accelerometer data */ + sdata = iio_priv(indio_dev); + if (sdata->enabled) { + if (indio_dev->active_scan_mask && + test_bit(0, indio_dev->active_scan_mask)) { + memcpy(&buffer[out_buf_index], &cdata->fifo_data[fifo_ptr], + LIS2HH12_FIFO_BYTE_FOR_SAMPLE); + out_buf_index += LIS2HH12_FIFO_BYTE_FOR_SAMPLE; + } + + if (indio_dev->scan_timestamp) { + offset = indio_dev->scan_bytes / sizeof(s64) - 1; + ((s64 *)buffer)[offset] = cdata->sensor_timestamp; + } + + iio_push_to_buffers(indio_dev, buffer); + } +} + +void lis2hh12_read_xyz(struct lis2hh12_data *cdata) +{ + int err; + + err = lis2hh12_read_register(cdata, LIS2HH12_OUTX_L_ADDR, + LIS2HH12_FIFO_BYTE_FOR_SAMPLE, cdata->fifo_data); + if (err < 0) + return; + + cdata->sensor_timestamp = cdata->timestamp; + lis2hh12_push_fifo_data(cdata, 0); +} + +void lis2hh12_read_fifo(struct lis2hh12_data *cdata, bool check_fifo_len) +{ + int err; + u8 fifo_src; + u16 read_len = cdata->fifo_size; + uint16_t i; + + if (!cdata->fifo_data) + return; + + if (check_fifo_len) { + err = lis2hh12_read_register(cdata, LIS2HH12_FIFO_STATUS_ADDR, 1, &fifo_src); + if (err < 0) + return; + + read_len = (fifo_src & LIS2HH12_FIFO_FSS_MASK); + read_len *= LIS2HH12_FIFO_BYTE_FOR_SAMPLE; + + if (read_len > cdata->fifo_size) + read_len = cdata->fifo_size; + } + + err = lis2hh12_read_register(cdata, LIS2HH12_OUTX_L_ADDR, read_len, + cdata->fifo_data); + if (err < 0) + return; + + for (i = 0; i < read_len; i += LIS2HH12_FIFO_BYTE_FOR_SAMPLE) { + lis2hh12_push_fifo_data(cdata, i); + cdata->sensor_timestamp += cdata->sensor_deltatime; + } +} + +static inline irqreturn_t lis2hh12_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int lis2hh12_trig_set_state(struct iio_trigger *trig, bool state) +{ + return 0; +} + +static int lis2hh12_buffer_preenable(struct iio_dev *indio_dev) +{ + int err; + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + err = lis2hh12_set_enable(sdata, true); + if (err < 0) + return err; + + return 0; +} + +static int lis2hh12_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + err = lis2hh12_set_enable(sdata, false); + if (err < 0) + return err; + + return 0; +} + +static const struct iio_buffer_setup_ops lis2hh12_buffer_setup_ops = { + .preenable = &lis2hh12_buffer_preenable, + .postdisable = &lis2hh12_buffer_postdisable, +}; + +int lis2hh12_allocate_rings(struct lis2hh12_data *cdata) +{ + int err, i; + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) { + err = iio_triggered_buffer_setup( + cdata->iio_sensors_dev[i], + &lis2hh12_handler_empty, + NULL, + &lis2hh12_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup; + } + + return 0; + +buffer_cleanup: + for (i--; i >= 0; i--) + iio_triggered_buffer_cleanup(cdata->iio_sensors_dev[i]); + + return err; +} + +void lis2hh12_deallocate_rings(struct lis2hh12_data *cdata) +{ + int i; + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) + iio_triggered_buffer_cleanup(cdata->iio_sensors_dev[i]); +} diff --git a/drivers/iio/stm/accel/st_lis2hh12_core.c b/drivers/iio/stm/accel/st_lis2hh12_core.c new file mode 100644 index 000000000000..109c1d7cccc3 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2hh12_core.c @@ -0,0 +1,899 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2hh12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "st_lis2hh12.h" +#include + +#define ST_LIS2HH12_DEV_ATTR_SAMP_FREQ() \ + IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \ + lis2hh12_sysfs_get_sampling_frequency, \ + lis2hh12_sysfs_set_sampling_frequency) + +#define ST_LIS2HH12_DEV_ATTR_SAMP_FREQ_AVAIL() \ + IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \ + lis2hh12_sysfs_sampling_frequency_avail) + +#define ST_LIS2HH12_DEV_ATTR_SCALE_AVAIL(name) \ + IIO_DEVICE_ATTR(name, S_IRUGO, \ + lis2hh12_sysfs_scale_avail, NULL , 0); + +#define LIS2HH12_ADD_CHANNEL(device_type, modif, index, mod, endian, sbits,\ + rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +struct lis2hh12_odr_reg { + u32 hz; + u8 value; +}; + +static const struct lis2hh12_odr_table_t { + u8 addr; + u8 mask; + struct lis2hh12_odr_reg odr_avl[LIS2HH12_ODR_LIST_NUM]; +} lis2hh12_odr_table = { + .addr = LIS2HH12_ODR_ADDR, + .mask = LIS2HH12_ODR_MASK, + + .odr_avl[0] = {.hz = 0,.value = LIS2HH12_ODR_POWER_DOWN_VAL,}, + .odr_avl[1] = {.hz = 10,.value = LIS2HH12_ODR_10HZ_VAL,}, + .odr_avl[2] = {.hz = 50,.value = LIS2HH12_ODR_50HZ_VAL,}, + .odr_avl[3] = {.hz = 100,.value = LIS2HH12_ODR_100HZ_VAL,}, + .odr_avl[4] = {.hz = 200,.value = LIS2HH12_ODR_200HZ_VAL,}, + .odr_avl[5] = {.hz = 400,.value = LIS2HH12_ODR_400HZ_VAL,}, + .odr_avl[6] = {.hz = 800,.value = LIS2HH12_ODR_800HZ_VAL,}, +}; + +struct lis2hh12_fs_reg { + unsigned int gain; + u8 value; +}; + +static struct lis2hh12_fs_table { + u8 addr; + u8 mask; + struct lis2hh12_fs_reg fs_avl[LIS2HH12_FS_LIST_NUM]; +} lis2hh12_fs_table = { + .addr = LIS2HH12_FS_ADDR, + .mask = LIS2HH12_FS_MASK, + .fs_avl[0] = { + .gain = LIS2HH12_FS_2G_GAIN, + .value = LIS2HH12_FS_2G_VAL, + }, + .fs_avl[1] = { + .gain = LIS2HH12_FS_4G_GAIN, + .value = LIS2HH12_FS_4G_VAL, + }, + .fs_avl[2] = { + .gain = LIS2HH12_FS_8G_GAIN, + .value = LIS2HH12_FS_8G_VAL, + }, +}; + +const struct iio_event_spec lis2hh12_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct lis2hh12_sensors_table { + const char *name; + const char *description; + const u32 min_odr_hz; + const u8 iio_channel_size; + const struct iio_chan_spec iio_channel[LIS2HH12_MAX_CHANNEL_SPEC]; +} lis2hh12_sensors_table[LIS2HH12_SENSORS_NUMB] = { + [LIS2HH12_ACCEL] = { + .name = "accel", + .description = "ST LIS2HH12 Accelerometer Sensor", + .min_odr_hz = 10, + .iio_channel = { + LIS2HH12_ADD_CHANNEL(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, LIS2HH12_OUTX_L_ADDR, 's'), + LIS2HH12_ADD_CHANNEL(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, LIS2HH12_OUTY_L_ADDR, 's'), + LIS2HH12_ADD_CHANNEL(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, LIS2HH12_OUTZ_L_ADDR, 's'), + ST_LIS2HH12_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) + }, + .iio_channel_size = LIS2HH12_MAX_CHANNEL_SPEC, + }, +}; + +inline int lis2hh12_read_register(struct lis2hh12_data *cdata, u8 reg_addr, int data_len, + u8 *data) +{ + return cdata->tf->read(cdata, reg_addr, data_len, data); +} + +static int lis2hh12_write_register(struct lis2hh12_data *cdata, u8 reg_addr, + u8 mask, u8 data) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = lis2hh12_read_register(cdata, reg_addr, 1, &old_data); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data); +} + +int lis2hh12_set_fifo_mode(struct lis2hh12_data *cdata, enum fifo_mode fm) +{ + int err; + u8 reg_value; + u8 set_bit = LIS2HH12_DIS_BIT; + + switch (fm) { + case BYPASS: + reg_value = LIS2HH12_FIFO_MODE_BYPASS; + set_bit = LIS2HH12_DIS_BIT; + break; + case STREAM: + reg_value = LIS2HH12_FIFO_MODE_STREAM; + set_bit = LIS2HH12_EN_BIT; + break; + default: + return -EINVAL; + } + + err = lis2hh12_write_register(cdata, LIS2HH12_FIFO_MODE_ADDR, + LIS2HH12_FIFO_MODE_MASK, reg_value); + + if (err < 0) + return err; + + cdata->sensor_timestamp = + iio_get_time_ns(cdata->iio_sensors_dev[LIS2HH12_ACCEL]); + + err = lis2hh12_write_register(cdata, LIS2HH12_FIFO_EN_ADDR, + LIS2HH12_FIFO_EN_MASK, set_bit); + if (err < 0) + return err; + + return 0; +} +EXPORT_SYMBOL(lis2hh12_set_fifo_mode); + +int lis2hh12_write_max_odr(struct lis2hh12_sensor_data *sdata) +{ + int err, i; + u32 max_odr = 0; + struct lis2hh12_sensor_data *t_sdata; + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) + if (CHECK_BIT(sdata->cdata->enabled_sensor, i)) { + t_sdata = iio_priv(sdata->cdata->iio_sensors_dev[i]); + if (t_sdata->odr > max_odr) + max_odr = t_sdata->odr; + } + + if (max_odr != sdata->cdata->common_odr) { + for (i = 0; i < LIS2HH12_ODR_LIST_NUM; i++) { + if (lis2hh12_odr_table.odr_avl[i].hz >= max_odr) + break; + } + if (i == LIS2HH12_ODR_LIST_NUM) + return -EINVAL; + + err = lis2hh12_write_register(sdata->cdata, + lis2hh12_odr_table.addr, + lis2hh12_odr_table.mask, + lis2hh12_odr_table.odr_avl[i].value); + if (err < 0) + return err; + + sdata->cdata->common_odr = max_odr; + sdata->cdata->sensor_deltatime = (max_odr) ? 1000000000L / max_odr : 0; + } + + return 0; +} + +int lis2hh12_set_fs(struct lis2hh12_sensor_data *sdata, unsigned int gain) +{ + int err, i; + + for (i = 0; i < LIS2HH12_FS_LIST_NUM; i++) { + if (lis2hh12_fs_table.fs_avl[i].gain == gain) + break; + } + + if (i == LIS2HH12_FS_LIST_NUM) + return -EINVAL; + + err = lis2hh12_write_register(sdata->cdata, + lis2hh12_fs_table.addr, lis2hh12_fs_table.mask, + lis2hh12_fs_table.fs_avl[i].value); + if (err < 0) + return err; + + sdata->gain = lis2hh12_fs_table.fs_avl[i].gain; + + return 0; +} + +int lis2hh12_update_drdy_irq(struct lis2hh12_sensor_data *sdata, bool state) +{ + u8 reg_addr, reg_val, reg_mask; + + switch (sdata->sindex) { + case LIS2HH12_ACCEL: + reg_addr = LIS2HH12_INT_CFG_ADDR; + if (sdata->cdata->hwfifo_enabled) + reg_mask = (LIS2HH12_INT_FTH_MASK); + else + reg_mask = (LIS2HH12_INT_DRDY_MASK); + + if (state) + reg_val = LIS2HH12_EN_BIT; + else + reg_val = LIS2HH12_DIS_BIT; + + break; + + default: + return -EINVAL; + } + + return lis2hh12_write_register(sdata->cdata, reg_addr, reg_mask, reg_val); +} +EXPORT_SYMBOL(lis2hh12_update_drdy_irq); + +static int lis2hh12_alloc_fifo(struct lis2hh12_data *cdata) +{ + int fifo_size; + + fifo_size = LIS2HH12_MAX_FIFO_LENGHT * LIS2HH12_FIFO_BYTE_FOR_SAMPLE; + + cdata->fifo_data = kmalloc(fifo_size, GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + + cdata->fifo_size = fifo_size; + + return 0; +} + +int lis2hh12_update_fifo_ths(struct lis2hh12_data *cdata, u8 fifo_len) +{ + int err; + struct iio_dev *indio_dev; + + indio_dev = cdata->iio_sensors_dev[LIS2HH12_ACCEL]; + + err = lis2hh12_write_register(cdata, LIS2HH12_FIFO_THS_ADDR, + LIS2HH12_FIFO_THS_MASK, + fifo_len); + if (err < 0) + return err; + + return 0; +} +EXPORT_SYMBOL(lis2hh12_update_fifo_ths); + +int lis2hh12_set_enable(struct lis2hh12_sensor_data *sdata, bool state) +{ + int err = 0; + u8 mode; + + if (sdata->enabled == state) + return 0; + + /* + * Start assuming the sensor enabled if state == true. + * It will be restored if an error occur. + */ + if (state) { + SET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + mode = STREAM; + } else { + RESET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + mode = BYPASS; + } + + /* Program the device */ + err = lis2hh12_update_drdy_irq(sdata, state); + if (err < 0) + goto enable_sensor_error; + + err = lis2hh12_set_fifo_mode(sdata->cdata, mode); + if (err < 0) + goto enable_sensor_error; + + err = lis2hh12_write_max_odr(sdata); + if (err < 0) + goto enable_sensor_error; + + sdata->enabled = state; + + return 0; + +enable_sensor_error: + if (state) { + RESET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + } else + SET_BIT(sdata->cdata->enabled_sensor, sdata->sindex); + + return err; +} +EXPORT_SYMBOL(lis2hh12_set_enable); + +int lis2hh12_init_sensors(struct lis2hh12_data *cdata) +{ + int err; + + /* + * Soft reset the device on power on. + */ + err = lis2hh12_write_register(cdata, LIS2HH12_SOFT_RESET_ADDR, + LIS2HH12_SOFT_RESET_MASK, + LIS2HH12_EN_BIT); + if (err < 0) + return err; + + mdelay(40); + + /* + * Enable latched interrupt mode on INT1. + */ + err = lis2hh12_write_register(cdata, LIS2HH12_LIR_ADDR, + LIS2HH12_LIR1_MASK, + LIS2HH12_EN_BIT); + if (err < 0) + return err; + + /* + * Enable block data update feature. + */ + err = lis2hh12_write_register(cdata, LIS2HH12_BDU_ADDR, + LIS2HH12_BDU_MASK, + LIS2HH12_EN_BIT); + if (err < 0) + return err; + + return 0; +} + +static ssize_t lis2hh12_sysfs_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct lis2hh12_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->odr); +} + +ssize_t lis2hh12_sysfs_set_sampling_frequency(struct device * dev, + struct device_attribute * attr, const char *buf, size_t count) +{ + int err; + u8 mode_count; + unsigned int odr, i; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + if (sdata->odr == odr) + return count; + + mode_count = LIS2HH12_ODR_LIST_NUM; + + for (i = 0; i < mode_count; i++) { + if (lis2hh12_odr_table.odr_avl[i].hz >= odr) + break; + } + if (i == LIS2HH12_ODR_LIST_NUM) + return -EINVAL; + + mutex_lock(&indio_dev->mlock); + sdata->odr = lis2hh12_odr_table.odr_avl[i].hz; + mutex_unlock(&indio_dev->mlock); + + err = lis2hh12_write_max_odr(sdata); + + return (err < 0) ? err : count; +} + +static ssize_t lis2hh12_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute + *attr, char *buf) +{ + int i, len = 0; + + for (i = 1; i < LIS2HH12_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + lis2hh12_odr_table.odr_avl[i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t lis2hh12_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < LIS2HH12_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + lis2hh12_fs_table.fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +ssize_t lis2hh12_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u64 event_type; + int64_t sensor_last_timestamp; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = sdata->cdata->sensor_timestamp; + + lis2hh12_read_fifo(sdata->cdata, true); + + if (sensor_last_timestamp == sdata->cdata->sensor_timestamp) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(IIO_ACCEL, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + sdata->cdata->sensor_timestamp); + + enable_irq(sdata->cdata->irq); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static ssize_t lis2hh12_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->hwfifo_enabled); +} + +ssize_t lis2hh12_sysfs_set_hwfifo_enabled(struct device * dev, + struct device_attribute * attr, const char *buf, size_t count) +{ + int err = 0, enable = 0; + u8 mode = BYPASS; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &enable); + if (err < 0) + return err; + + if (enable != 0x0 && enable != 0x1) + return -EINVAL; + + mode = (enable == 0x0) ? BYPASS : STREAM; + + err = lis2hh12_set_fifo_mode(sdata->cdata, mode); + if (err < 0) + return err; + + sdata->cdata->hwfifo_enabled = enable; + + return count; +} + +static ssize_t lis2hh12_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->hwfifo_watermark); +} + +ssize_t lis2hh12_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, const char *buf, size_t count) +{ + int err = 0, watermark = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > LIS2HH12_MAX_FIFO_THS)) + return -EINVAL; + + err = lis2hh12_update_fifo_ths(sdata->cdata, watermark); + if (err < 0) + return err; + + sdata->cdata->hwfifo_watermark = watermark; + + return count; +} + +static ssize_t lis2hh12_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +static ssize_t lis2hh12_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", LIS2HH12_MAX_FIFO_THS); +} + +static int lis2hh12_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[2], nbytes; + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = lis2hh12_set_enable(sdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + msleep(40); + + nbytes = ch->scan_type.realbits / 8; + + err = lis2hh12_read_register(sdata->cdata, ch->address, nbytes, outdata); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + err = lis2hh12_set_enable(sdata, false); + mutex_unlock(&indio_dev->mlock); + + if (err < 0) + return err; + + return IIO_VAL_INT; + + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->gain; + + return IIO_VAL_INT_PLUS_MICRO; + + default: + return -EINVAL; + } + + return 0; +} + +static int lis2hh12_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long mask) +{ + int err, i; + struct lis2hh12_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + for (i = 0; i < LIS2HH12_FS_LIST_NUM; i++) { + if (lis2hh12_fs_table.fs_avl[i].gain == val2) + break; + } + + err = lis2hh12_set_fs(sdata, lis2hh12_fs_table.fs_avl[i].gain); + mutex_unlock(&indio_dev->mlock); + + break; + + default: + return -EINVAL; + } + + return err; +} + +static ST_LIS2HH12_DEV_ATTR_SAMP_FREQ(); +static ST_LIS2HH12_DEV_ATTR_SAMP_FREQ_AVAIL(); +static ST_LIS2HH12_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); + +static ST_LIS2HH12_HWFIFO_ENABLED(); +static ST_LIS2HH12_HWFIFO_WATERMARK(); +static ST_LIS2HH12_HWFIFO_WATERMARK_MIN(); +static ST_LIS2HH12_HWFIFO_WATERMARK_MAX(); +static ST_LIS2HH12_HWFIFO_FLUSH(); + +static struct attribute *lis2hh12_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group lis2hh12_accel_attribute_group = { + .attrs = lis2hh12_accel_attributes, +}; + +static const struct iio_info lis2hh12_info[LIS2HH12_SENSORS_NUMB] = { + [LIS2HH12_ACCEL] = { + .attrs = &lis2hh12_accel_attribute_group, + .read_raw = &lis2hh12_read_raw, + .write_raw = &lis2hh12_write_raw, + }, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops lis2hh12_trigger_ops = { + .set_trigger_state = (&lis2hh12_trig_set_state), +}; +#define LIS2HH12_TRIGGER_OPS (&lis2hh12_trigger_ops) +#else /*CONFIG_IIO_TRIGGER */ +#define LIS2HH12_TRIGGER_OPS NULL +#endif /*CONFIG_IIO_TRIGGER */ + +#ifdef CONFIG_OF +static u32 lis2hh12_parse_dt(struct lis2hh12_data *cdata) +{ + u32 val; + struct device_node *np; + + np = cdata->dev->of_node; + if (!np) + return -EINVAL; + /*TODO for this device interrupt pin is only one!!*/ + if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && + (val <= 1) && (val > 0)) + cdata->drdy_int_pin = (u8) val; + else + cdata->drdy_int_pin = 1; + + return 0; +} +#endif /*CONFIG_OF */ + +int lis2hh12_common_probe(struct lis2hh12_data *cdata, int irq) +{ + u8 wai = 0; + int32_t err, i, n; + struct iio_dev *piio_dev; + struct lis2hh12_sensor_data *sdata; + + mutex_init(&cdata->tb.buf_lock); + + cdata->fifo_data = 0; + cdata->hwfifo_enabled = 0; + cdata->hwfifo_watermark = 0; + + err = lis2hh12_read_register(cdata, LIS2HH12_WHO_AM_I_ADDR, 1, &wai); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + if (wai != LIS2HH12_WHO_AM_I_DEF) { + dev_err(cdata->dev, "Who-Am-I value not valid.\n"); + + return -ENODEV; + } + + if (irq > 0) { + cdata->irq = irq; +#ifdef CONFIG_OF + err = lis2hh12_parse_dt(cdata); + if (err < 0) + return err; +#else /* CONFIG_OF */ + if (cdata->dev->platform_data) { + cdata->drdy_int_pin = ((struct lis2hh12_platform_data *) + cdata->dev->platform_data)->drdy_int_pin; + + if ((cdata->drdy_int_pin > 1) || (cdata->drdy_int_pin < 1)) + cdata->drdy_int_pin = 1; + } else + cdata->drdy_int_pin = 1; +#endif /* CONFIG_OF */ + + dev_info(cdata->dev, "driver use DRDY int pin %d\n", + cdata->drdy_int_pin); + } + + cdata->common_odr = 0; + cdata->enabled_sensor = 0; + + err = lis2hh12_alloc_fifo(cdata); + if (err) + return err; + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) { + piio_dev = devm_iio_device_alloc(cdata->dev, + sizeof(struct lis2hh12_sensor_data *)); + if (piio_dev == NULL) { + err = -ENOMEM; + + goto iio_device_free; + } + + cdata->iio_sensors_dev[i] = piio_dev; + sdata = iio_priv(piio_dev); + sdata->enabled = false; + sdata->cdata = cdata; + sdata->sindex = i; + sdata->name = lis2hh12_sensors_table[i].name; + sdata->odr = lis2hh12_sensors_table[i].min_odr_hz; + sdata->gain = lis2hh12_fs_table.fs_avl[0].gain; + + piio_dev->channels = lis2hh12_sensors_table[i].iio_channel; + piio_dev->num_channels = lis2hh12_sensors_table[i].iio_channel_size; + piio_dev->info = &lis2hh12_info[i]; + piio_dev->modes = INDIO_DIRECT_MODE; + piio_dev->name = kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + sdata->name); + } + + err = lis2hh12_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto iio_device_free; + + err = lis2hh12_init_sensors(cdata); + if (err < 0) + goto iio_device_free; + + err = lis2hh12_allocate_rings(cdata); + if (err < 0) + goto iio_device_free; + + if (irq > 0) { + err = lis2hh12_allocate_triggers(cdata, LIS2HH12_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < LIS2HH12_SENSORS_NUMB; n++) { + err = iio_device_register(cdata->iio_sensors_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + dev_info(cdata->dev, "%s: probed\n", LIS2HH12_DEV_NAME); + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->iio_sensors_dev[n]); + +deallocate_ring: + lis2hh12_deallocate_rings(cdata); + +iio_device_free: + for (i--; i >= 0; i--) + iio_device_free(cdata->iio_sensors_dev[i]); + + return err; +} +EXPORT_SYMBOL(lis2hh12_common_probe); + +void lis2hh12_common_remove(struct lis2hh12_data *cdata, int irq) +{ + int i; + + if (cdata->fifo_data) { + kfree(cdata->fifo_data); + cdata->fifo_size = 0; + } + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) + iio_device_unregister(cdata->iio_sensors_dev[i]); + + if (irq > 0) + lis2hh12_deallocate_triggers(cdata); + + lis2hh12_deallocate_rings(cdata); + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) + iio_device_free(cdata->iio_sensors_dev[i]); +} + +EXPORT_SYMBOL(lis2hh12_common_remove); + +#ifdef CONFIG_PM +int __maybe_unused lis2hh12_common_suspend(struct lis2hh12_data *cdata) +{ + return 0; +} + +EXPORT_SYMBOL(lis2hh12_common_suspend); + +int __maybe_unused lis2hh12_common_resume(struct lis2hh12_data *cdata) +{ + return 0; +} + +EXPORT_SYMBOL(lis2hh12_common_resume); +#endif /* CONFIG_PM */ + +MODULE_DESCRIPTION("STMicroelectronics lis2hh12 driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2hh12_i2c.c b/drivers/iio/stm/accel/st_lis2hh12_i2c.c new file mode 100644 index 000000000000..fd161a1ee3cd --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2hh12_i2c.c @@ -0,0 +1,157 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2hh12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lis2hh12.h" + +static int lis2hh12_i2c_read(struct lis2hh12_data *cdata, u8 reg_addr, int len, + u8 * data) +{ + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return(i2c_transfer(client->adapter, msg, 2)); +} + +static int lis2hh12_i2c_write(struct lis2hh12_data *cdata, + u8 reg_addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + return(i2c_transfer(client->adapter, &msg, 1)); +} + +static const struct lis2hh12_transfer_function lis2hh12_tf_i2c = { + .write = lis2hh12_i2c_write, + .read = lis2hh12_i2c_read, +}; + +static int lis2hh12_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct lis2hh12_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + cdata->tf = &lis2hh12_tf_i2c; + i2c_set_clientdata(client, cdata); + + err = lis2hh12_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int lis2hh12_i2c_remove(struct i2c_client *client) +{ + struct lis2hh12_data *cdata = i2c_get_clientdata(client); + + lis2hh12_common_remove(cdata, client->irq); + dev_info(cdata->dev, "%s: removed\n", LIS2HH12_DEV_NAME); + kfree(cdata); + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused lis2hh12_suspend(struct device *dev) +{ + struct lis2hh12_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return lis2hh12_common_suspend(cdata); +} + +static int __maybe_unused lis2hh12_resume(struct device *dev) +{ + struct lis2hh12_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return lis2hh12_common_resume(cdata); +} + +static const struct dev_pm_ops lis2hh12_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(lis2hh12_suspend, lis2hh12_resume) +}; + +#define LIS2HH12_PM_OPS (&lis2hh12_pm_ops) +#else /* CONFIG_PM */ +#define LIS2HH12_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id lis2hh12_ids[] = { + {"lis2hh12", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, lis2hh12_ids); + +#ifdef CONFIG_OF +static const struct of_device_id lis2hh12_id_table[] = { + {.compatible = "st,lis2hh12",}, + {}, +}; + +MODULE_DEVICE_TABLE(of, lis2hh12_id_table); +#endif /* CONFIG_OF */ + +static struct i2c_driver lis2hh12_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = LIS2HH12_DEV_NAME, + .pm = LIS2HH12_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = lis2hh12_id_table, +#endif /* CONFIG_OF */ + }, + .probe = lis2hh12_i2c_probe, + .remove = lis2hh12_i2c_remove, + .id_table = lis2hh12_ids, +}; + +module_i2c_driver(lis2hh12_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics lis2hh12 i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2hh12_spi.c b/drivers/iio/stm/accel/st_lis2hh12_spi.c new file mode 100644 index 000000000000..d8e7f41ad2c5 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2hh12_spi.c @@ -0,0 +1,184 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2hh12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lis2hh12.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int lis2hh12_spi_read(struct lis2hh12_data *cdata, + u8 reg_addr, int len, u8 *data) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + mutex_lock(&cdata->tb.buf_lock); + + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + + mutex_unlock(&cdata->tb.buf_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + + return err; +} + +static int lis2hh12_spi_write(struct lis2hh12_data *cdata, + u8 reg_addr, int len, u8 *data) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= LIS2HH12_TX_MAX_LENGTH) + return -ENOMEM; + + mutex_lock(&cdata->tb.buf_lock); + + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + + mutex_unlock(&cdata->tb.buf_lock); + + return err; +} + +static const struct lis2hh12_transfer_function lis2hh12_tf_spi = { + .write = lis2hh12_spi_write, + .read = lis2hh12_spi_read, +}; + +static int lis2hh12_spi_probe(struct spi_device *spi) +{ + int err; + struct lis2hh12_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + cdata->tf = &lis2hh12_tf_spi; + spi_set_drvdata(spi, cdata); + + err = lis2hh12_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int lis2hh12_spi_remove(struct spi_device *spi) +{ + struct lis2hh12_data *cdata = spi_get_drvdata(spi); + + lis2hh12_common_remove(cdata, spi->irq); + dev_info(cdata->dev, "%s: removed\n", LIS2HH12_DEV_NAME); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused lis2hh12_suspend(struct device *dev) +{ + struct lis2hh12_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return lis2hh12_common_suspend(cdata); +} + +static int __maybe_unused lis2hh12_resume(struct device *dev) +{ + struct lis2hh12_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return lis2hh12_common_resume(cdata); +} + +static const struct dev_pm_ops lis2hh12_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(lis2hh12_suspend, lis2hh12_resume) +}; + +#define LIS2HH12_PM_OPS (&lis2hh12_pm_ops) +#else /* CONFIG_PM */ +#define LIS2HH12_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id lis2hh12_ids[] = { + {"lis2hh12", 0}, + {} +}; + +MODULE_DEVICE_TABLE(spi, lis2hh12_ids); + +#ifdef CONFIG_OF +static const struct of_device_id lis2hh12_id_table[] = { + { .compatible = "st,lis2hh12"}, + {}, +}; + +MODULE_DEVICE_TABLE(of, lis2hh12_id_table); +#endif /* CONFIG_OF */ + +static struct spi_driver lis2hh12_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = LIS2HH12_DEV_NAME, + .pm = LIS2HH12_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = lis2hh12_id_table, +#endif /* CONFIG_OF */ + }, + .probe = lis2hh12_spi_probe, + .remove = lis2hh12_spi_remove, + .id_table = lis2hh12_ids, +}; + +module_spi_driver(lis2hh12_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics lis2hh12 spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis2hh12_trigger.c b/drivers/iio/stm/accel/st_lis2hh12_trigger.c new file mode 100644 index 000000000000..e7491999f34b --- /dev/null +++ b/drivers/iio/stm/accel/st_lis2hh12_trigger.c @@ -0,0 +1,110 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lis2hh12 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis2hh12.h" + +static irqreturn_t lis2hh12_irq_management(int irq, void *private) +{ + struct lis2hh12_data *cdata = private; + u8 status; + + cdata->timestamp = + iio_get_time_ns(cdata->iio_sensors_dev[LIS2HH12_ACCEL]); + + if (cdata->hwfifo_enabled) { + cdata->tf->read(cdata, LIS2HH12_FIFO_STATUS_ADDR, 1, &status); + + if (status & LIS2HH12_FIFO_SRC_FTH_MASK) + lis2hh12_read_fifo(cdata, true); + } else { + cdata->tf->read(cdata, LIS2HH12_STATUS_ADDR, 1, &status); + + if (status & LIS2HH12_DATA_XYZ_RDY) + lis2hh12_read_xyz(cdata); + } + + return IRQ_HANDLED; +} + +int lis2hh12_allocate_triggers(struct lis2hh12_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->iio_trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->iio_sensors_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->iio_trig[i] = iio_trigger_alloc("%s-trigger", + cdata->iio_sensors_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->iio_trig[i]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + err = -ENOMEM; + + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->iio_trig[i], + cdata->iio_sensors_dev[i]); + cdata->iio_trig[i]->ops = trigger_ops; + cdata->iio_trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, NULL, lis2hh12_irq_management, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < LIS2HH12_SENSORS_NUMB; n++) { + err = iio_trigger_register(cdata->iio_trig[n]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + + goto free_irq; + } + cdata->iio_sensors_dev[n]->trig = cdata->iio_trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->iio_trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->iio_trig[i]); + + return err; +} +EXPORT_SYMBOL(lis2hh12_allocate_triggers); + +void lis2hh12_deallocate_triggers(struct lis2hh12_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < LIS2HH12_SENSORS_NUMB; i++) + iio_trigger_unregister(cdata->iio_trig[i]); +} +EXPORT_SYMBOL(lis2hh12_deallocate_triggers); diff --git a/drivers/iio/stm/accel/st_lis3dhh.c b/drivers/iio/stm/accel/st_lis3dhh.c new file mode 100644 index 000000000000..540f65e97931 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis3dhh.c @@ -0,0 +1,430 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis3dhh sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lis3dhh.h" + +#define LIS3DHH_DEV_NAME "lis3dhh" +#define IIS3DHHC_DEV_NAME "iis3dhhc" + +#define REG_WHOAMI_ADDR 0x0f +#define REG_WHOAMI_VAL 0x11 + +#define REG_CTRL1_ADDR 0x20 +#define REG_CTRL1_BDU_MASK BIT(0) +#define REG_CTRL1_SW_RESET_MASK BIT(2) +#define REG_CTRL1_EN_MASK BIT(7) + +#define REG_INT1_CTRL_ADDR 0x21 +#define REG_INT2_CTRL_ADDR 0x22 +#define REG_INT_FTM_MASK BIT(3) + +#define ST_LIS3DHH_FS IIO_G_TO_M_S_2(76) + +#define ST_LIS3DHH_DATA_CHANNEL(addr, modx, scan_idx) \ +{ \ + .type = IIO_ACCEL, \ + .address = addr, \ + .modified = 1, \ + .channel2 = modx, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +#define ST_LIS3DHH_FLUSH_CHANNEL() \ +{ \ + .type = IIO_ACCEL, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lis3dhh_fifo_flush_event, \ + .num_event_specs = 1, \ +} + +const struct iio_event_spec st_lis3dhh_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lis3dhh_channels[] = { + ST_LIS3DHH_DATA_CHANNEL(0x28, IIO_MOD_X, 0), + ST_LIS3DHH_DATA_CHANNEL(0x2a, IIO_MOD_Y, 1), + ST_LIS3DHH_DATA_CHANNEL(0x2c, IIO_MOD_Z, 2), + ST_LIS3DHH_FLUSH_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +#define SENSORS_SPI_READ BIT(7) + int st_lis3dhh_spi_read(struct st_lis3dhh_hw *hw, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(hw->dev); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_lis3dhh_spi_write(struct st_lis3dhh_hw *hw, u8 addr, + int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(hw->dev); + + if (len >= ST_LIS3DHH_TX_MAX_LENGTH) + return -ENOMEM; + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +int st_lis3dhh_write_with_mask(struct st_lis3dhh_hw *hw, u8 addr, u8 mask, + u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = st_lis3dhh_spi_read(hw, addr, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + mutex_unlock(&hw->lock); + + return err; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = st_lis3dhh_spi_write(hw, addr, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to write %02x register\n", addr); + mutex_unlock(&hw->lock); + + return err; + } + + mutex_unlock(&hw->lock); + + return 0; +} + +int st_lis3dhh_set_enable(struct st_lis3dhh_hw *hw, bool enable) +{ + return st_lis3dhh_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_EN_MASK, enable); +} + +static int st_lis3dhh_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + int err, delay; + u8 data[2]; + + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + + err = st_lis3dhh_set_enable(hw, true); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + /* sample to discard, 3 * odr us */ + delay = 3000000 / ST_LIS3DHH_ODR; + usleep_range(delay, delay + 1); + + err = st_lis3dhh_spi_read(hw, ch->address, 2, data); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + err = st_lis3dhh_set_enable(hw, false); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(data); + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&iio_dev->mlock); + + ret = IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = ST_LIS3DHH_FS; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = ST_LIS3DHH_ODR; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static ssize_t +st_lis3dhh_get_sampling_frequency_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "%d\n", ST_LIS3DHH_ODR); +} + +static ssize_t st_lis3dhh_get_scale_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "0.%06d\n", (int)ST_LIS3DHH_FS); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lis3dhh_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lis3dhh_get_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_lis3dhh_get_hwfifo_watermark, + st_lis3dhh_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lis3dhh_get_max_hwfifo_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lis3dhh_flush_hwfifo, 0); + +static struct attribute *st_lis3dhh_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.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis3dhh_attribute_group = { + .attrs = st_lis3dhh_attributes, +}; + +static const struct iio_info st_lis3dhh_info = { + .attrs = &st_lis3dhh_attribute_group, + .read_raw = st_lis3dhh_read_raw, +}; + +static int st_lis3dhh_check_whoami(struct st_lis3dhh_hw *hw) +{ + u8 data; + int err; + + err = st_lis3dhh_spi_read(hw, REG_WHOAMI_ADDR, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != REG_WHOAMI_VAL) { + dev_err(hw->dev, "wrong whoami {%02x-%02x}\n", + data, REG_WHOAMI_VAL); + return -ENODEV; + } + + return 0; +} + +static int st_lis3dhh_of_get_drdy_pin(struct st_lis3dhh_hw *hw, int *drdy_pin) +{ + struct device_node *np = hw->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin); +} + +static int st_lis3dhh_set_drdy_reg(struct st_lis3dhh_hw *hw) +{ + int drdy_pin; + u8 drdy_reg; + + if (st_lis3dhh_of_get_drdy_pin(hw, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + drdy_reg = REG_INT1_CTRL_ADDR; + break; + case 2: + drdy_reg = REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported data ready pin\n"); + return -EINVAL; + } + + return st_lis3dhh_write_with_mask(hw, drdy_reg, REG_INT_FTM_MASK, 1); +} + +static int st_lis3dhh_init_device(struct st_lis3dhh_hw *hw) +{ + int err; + + err = st_lis3dhh_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_SW_RESET_MASK, 1); + if (err < 0) + return err; + + msleep(200); + + err = st_lis3dhh_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_BDU_MASK, 1); + if (err < 0) + return err; + + err = st_lis3dhh_update_watermark(hw, hw->watermark); + if (err < 0) + return err; + + return st_lis3dhh_set_drdy_reg(hw); +} + +static int st_lis3dhh_spi_probe(struct spi_device *spi) +{ + struct st_lis3dhh_hw *hw; + struct iio_dev *iio_dev; + int err; + + iio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*hw)); + if (!iio_dev) + return -ENOMEM; + + spi_set_drvdata(spi, iio_dev); + + iio_dev->channels = st_lis3dhh_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis3dhh_channels); + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->info = &st_lis3dhh_info; + iio_dev->dev.parent = &spi->dev; + iio_dev->name = spi->modalias; + + hw = iio_priv(iio_dev); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->lock); + + hw->watermark = 1; + hw->dev = &spi->dev; + hw->name = spi->modalias; + hw->irq = spi->irq; + hw->iio_dev = iio_dev; + + err = st_lis3dhh_check_whoami(hw); + if (err < 0) + return err; + + err = st_lis3dhh_init_device(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_lis3dhh_fifo_setup(hw); + if (err < 0) + return err; + } + + return devm_iio_device_register(hw->dev, iio_dev); +} + +static const struct of_device_id st_lis3dhh_spi_of_match[] = { + { + .compatible = "st,lis3dhh", + .data = LIS3DHH_DEV_NAME, + }, + { + .compatible = "st,iis3dhhc", + .data = IIS3DHHC_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lis3dhh_spi_of_match); + +static const struct spi_device_id st_lis3dhh_spi_id_table[] = { + { LIS3DHH_DEV_NAME }, + { IIS3DHHC_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lis3dhh_spi_id_table); + +static struct spi_driver st_lis3dhh_driver = { + .driver = { + .name = "st_lis3dhh", + .of_match_table = of_match_ptr(st_lis3dhh_spi_of_match), + }, + .probe = st_lis3dhh_spi_probe, + .id_table = st_lis3dhh_spi_id_table, +}; +module_spi_driver(st_lis3dhh_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lis3dhh sensor driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis3dhh.h b/drivers/iio/stm/accel/st_lis3dhh.h new file mode 100644 index 000000000000..16bc4bca7d14 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis3dhh.h @@ -0,0 +1,62 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lis3dhh sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_LIS3DHH_H +#define ST_LIS3DHH_H + + +#include + +#define ST_LIS3DHH_DATA_SIZE 6 +#define ST_LIS3DHH_RX_MAX_LENGTH 96 +#define ST_LIS3DHH_TX_MAX_LENGTH 8 + +#define ST_LIS3DHH_ODR 1100 + +struct st_lis3dhh_transfer_buffer { + u8 rx_buf[ST_LIS3DHH_RX_MAX_LENGTH]; + u8 tx_buf[ST_LIS3DHH_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_lis3dhh_hw { + struct device *dev; + const char *name; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + + u8 watermark; + s64 delta_ts; + s64 ts_irq; + s64 ts; + struct iio_dev *iio_dev; + struct st_lis3dhh_transfer_buffer tb; +}; + +int st_lis3dhh_write_with_mask(struct st_lis3dhh_hw *hw, u8 addr, u8 mask, + u8 val); + int st_lis3dhh_spi_read(struct st_lis3dhh_hw *hw, u8 addr, int len, u8 *data); +int st_lis3dhh_set_enable(struct st_lis3dhh_hw *hw, bool enable); +int st_lis3dhh_fifo_setup(struct st_lis3dhh_hw *hw); +ssize_t st_lis3dhh_flush_hwfifo(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lis3dhh_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lis3dhh_get_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + char *buf); +ssize_t st_lis3dhh_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +int st_lis3dhh_update_watermark(struct st_lis3dhh_hw *hw, u8 watermark); + +#endif /* ST_LIS3DHH_H */ diff --git a/drivers/iio/stm/accel/st_lis3dhh_buffer.c b/drivers/iio/stm/accel/st_lis3dhh_buffer.c new file mode 100644 index 000000000000..7bb00922a963 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis3dhh_buffer.c @@ -0,0 +1,282 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis3dhh sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis3dhh.h" + +#define REG_CTRL3_ADDR 0x23 +#define REG_CTRL5_ACC_FIFO_EN_MASK BIT(1) + +#define REG_FIFO_CTRL_REG 0x2e +#define REG_FIFO_CTRL_REG_WTM_MASK GENMASK(4, 0) +#define REG_FIFO_CTRL_MODE_MASK GENMASK(7, 5) + +#define REG_FIFO_SRC_ADDR 0x2f +#define REG_FIFO_SRC_OVR_MASK BIT(6) +#define REG_FIFO_SRC_FSS_MASK GENMASK(5, 0) + +#define ST_LIS3DHH_MAX_WATERMARK 28 + +enum st_lis3dhh_fifo_mode { + ST_LIS3DHH_FIFO_BYPASS = 0x0, + ST_LIS3DHH_FIFO_STREAM = 0x6, +}; + +static inline s64 st_lis3dhh_get_timestamp(struct st_lis3dhh_hw *hw) +{ + return iio_get_time_ns(hw->iio_dev); +} + +#define ST_LIS3DHH_EWMA_LEVEL 120 +#define ST_LIS3DHH_EWMA_DIV 128 +static inline s64 st_lis3dhh_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LIS3DHH_EWMA_DIV - weight) * diff, + ST_LIS3DHH_EWMA_DIV); + + return old + incr; +} + +static int st_lis3dhh_set_fifo_mode(struct st_lis3dhh_hw *hw, + enum st_lis3dhh_fifo_mode mode) +{ + return st_lis3dhh_write_with_mask(hw, REG_FIFO_CTRL_REG, + REG_FIFO_CTRL_MODE_MASK, mode); +} + +static int st_lis3dhh_read_fifo(struct st_lis3dhh_hw *hw) +{ + u8 iio_buff[ALIGN(ST_LIS3DHH_DATA_SIZE, sizeof(s64)) + sizeof(s64)]; + u8 buff[ST_LIS3DHH_RX_MAX_LENGTH], data, nsamples; + struct iio_dev *iio_dev = hw->iio_dev; + struct iio_chan_spec const *ch = iio_dev->channels; + int i, err, word_len, fifo_len, read_len = 0; + + err = st_lis3dhh_spi_read(hw, REG_FIFO_SRC_ADDR, sizeof(data), &data); + if (err < 0) + return err; + + nsamples = data & REG_FIFO_SRC_FSS_MASK; + fifo_len = nsamples * ST_LIS3DHH_DATA_SIZE; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buff)); + err = st_lis3dhh_spi_read(hw, ch[0].address, word_len, buff); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_LIS3DHH_DATA_SIZE) { + memcpy(iio_buff, &buff[i], ST_LIS3DHH_DATA_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buff, + hw->ts); + hw->ts += hw->delta_ts; + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lis3dhh_flush_hwfifo(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + s64 code; + int err; + + mutex_lock(&hw->fifo_lock); + + err = st_lis3dhh_read_fifo(hw); + hw->ts_irq = st_lis3dhh_get_timestamp(hw); + + mutex_unlock(&hw->fifo_lock); + + code = IIO_UNMOD_EVENT_CODE(IIO_ACCEL, -1, + IIO_EV_TYPE_FIFO_FLUSH, + IIO_EV_DIR_EITHER); + iio_push_event(iio_dev, code, hw->ts_irq); + + return err < 0 ? err : size; +} + +ssize_t st_lis3dhh_get_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", hw->watermark); +} + +int st_lis3dhh_update_watermark(struct st_lis3dhh_hw *hw, u8 watermark) +{ + return st_lis3dhh_write_with_mask(hw, REG_FIFO_CTRL_REG, + REG_FIFO_CTRL_REG_WTM_MASK, + watermark); +} + +ssize_t st_lis3dhh_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + int err, val; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock; + } + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto unlock; + + if (val < 1 || val > ST_LIS3DHH_MAX_WATERMARK) { + err = -EINVAL; + goto unlock; + } + + err = st_lis3dhh_update_watermark(hw, val); + if (err < 0) + goto unlock; + + hw->watermark = val; + +unlock: + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : size; +} + +ssize_t st_lis3dhh_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LIS3DHH_MAX_WATERMARK); +} + +static irqreturn_t st_lis3dhh_buffer_handler_irq(int irq, void *private) +{ + struct st_lis3dhh_hw *hw = private; + s64 ts, delta_ts; + + ts = st_lis3dhh_get_timestamp(hw); + delta_ts = div_s64(ts - hw->ts_irq, hw->watermark); + hw->delta_ts = st_lis3dhh_ewma(hw->delta_ts, delta_ts, + ST_LIS3DHH_EWMA_LEVEL); + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lis3dhh_buffer_handler_thread(int irq, void *private) +{ + struct st_lis3dhh_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_lis3dhh_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_lis3dhh_update_fifo(struct st_lis3dhh_hw *hw, bool enable) +{ + enum st_lis3dhh_fifo_mode mode; + int err; + + if (enable) { + hw->ts_irq = hw->ts = st_lis3dhh_get_timestamp(hw); + hw->delta_ts = div_s64(1000000000LL, ST_LIS3DHH_ODR); + } + + err = st_lis3dhh_write_with_mask(hw, REG_CTRL3_ADDR, + REG_CTRL5_ACC_FIFO_EN_MASK, enable); + if (err < 0) + return err; + + mode = enable ? ST_LIS3DHH_FIFO_STREAM : ST_LIS3DHH_FIFO_BYPASS; + err = st_lis3dhh_set_fifo_mode(hw, mode); + if (err < 0) + return err; + + return st_lis3dhh_set_enable(hw, enable); +} + +static int st_lis3dhh_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lis3dhh_update_fifo(iio_priv(iio_dev), true); +} + +static int st_lis3dhh_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lis3dhh_update_fifo(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_lis3dhh_buffer_ops = { + .preenable = st_lis3dhh_buffer_preenable, + .postdisable = st_lis3dhh_buffer_postdisable, +}; + +int st_lis3dhh_fifo_setup(struct st_lis3dhh_hw *hw) +{ + struct iio_dev *iio_dev = hw->iio_dev; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + int ret; + + ret = devm_request_threaded_irq(hw->dev, hw->irq, + st_lis3dhh_buffer_handler_irq, + st_lis3dhh_buffer_handler_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + hw->name, hw); + if (ret) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return ret; + } + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + ret = devm_iio_kfifo_buffer_setup(hw->dev, iio_dev, + INDIO_BUFFER_SOFTWARE, + &st_lis3dhh_buffer_ops); + if (ret) + return ret; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(iio_dev, buffer); + iio_dev->modes |= INDIO_BUFFER_SOFTWARE; + iio_dev->setup_ops = &st_lis3dhh_buffer_ops; +#endif /* LINUX_VERSION_CODE */ + + return 0; +} diff --git a/drivers/iio/stm/imu/Kconfig b/drivers/iio/stm/imu/Kconfig new file mode 100644 index 000000000000..89c984c46852 --- /dev/null +++ b/drivers/iio/stm/imu/Kconfig @@ -0,0 +1,20 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# IIO imu drivers configuration +# +# When adding new entries keep the list in alphabetical order + +menu "Inertial measurement units" + +source "drivers/iio/stm/imu/st_lsm6dsox/Kconfig" +source "drivers/iio/stm/imu/st_lsm6dsrx/Kconfig" +source "drivers/iio/stm/imu/st_ism330dhcx/Kconfig" +source "drivers/iio/stm/imu/st_asm330lhhx/Kconfig" +source "drivers/iio/stm/imu/st_imu68/Kconfig" +source "drivers/iio/stm/imu/st_ism330dlc/Kconfig" +source "drivers/iio/stm/imu/st_lsm6ds3/Kconfig" +source "drivers/iio/stm/imu/st_lsm6ds3h/Kconfig" +source "drivers/iio/stm/imu/st_lsm6dsm/Kconfig" +source "drivers/iio/stm/imu/st_lsm6dsvx/Kconfig" + +endmenu diff --git a/drivers/iio/stm/imu/Makefile b/drivers/iio/stm/imu/Makefile new file mode 100644 index 000000000000..ce18e5917588 --- /dev/null +++ b/drivers/iio/stm/imu/Makefile @@ -0,0 +1,16 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for Inertial Measurement Units +# + +# When adding new entries keep the list in alphabetical order +obj-y += st_lsm6dsox/ +obj-y += st_lsm6dsrx/ +obj-y += st_ism330dhcx/ +obj-y += st_asm330lhhx/ +obj-y += st_imu68/ +obj-y += st_ism330dlc/ +obj-y += st_lsm6ds3/ +obj-y += st_lsm6ds3h/ +obj-y += st_lsm6dsm/ +obj-y += st_lsm6dsvx/ diff --git a/drivers/iio/stm/imu/st_asm330lhhx/Kconfig b/drivers/iio/stm/imu/st_asm330lhhx/Kconfig new file mode 100644 index 000000000000..0e8506bc4b00 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/Kconfig @@ -0,0 +1,45 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_ASM330LHHX + tristate "STMicroelectronics ASM330LHHX sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_TRIGGERED_BUFFER + select IIO_ST_ASM330LHHX_I2C if (I2C) + select IIO_ST_ASM330LHHX_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics ASM330LHH/ASM330LHHX imu + sensors. + + To compile this driver as a module, choose M here: the module + will be called st_asm330lhhx. + +config IIO_ST_ASM330LHHX_I2C + tristate + select REGMAP_I2C + depends on IIO_ST_ASM330LHHX + +config IIO_ST_ASM330LHHX_SPI + tristate + select REGMAP_SPI + depends on IIO_ST_ASM330LHHX + +config IIO_ST_ASM330LHHX_EN_BASIC_FEATURES + bool "Enable internal basic features event detection" + depends on IIO_ST_ASM330LHHX + help + Enable internal basic features event detection sensor + +config IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP + bool "Enable async hw timestamp read" + depends on IIO_ST_ASM330LHHX + help + Enable async task that sends over hw timestamp events. + +config IIO_ST_ASM330LHHX_MLC_PRELOAD + bool "Preload some examples on MLC/FSM core" + depends on IIO_ST_ASM330LHHX + help + Preload some examples on machine learning core and finite state + machine. The examples code are hardcoded in to the driver. diff --git a/drivers/iio/stm/imu/st_asm330lhhx/Makefile b/drivers/iio/stm/imu/st_asm330lhhx/Makefile new file mode 100644 index 000000000000..9809253c64e0 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/Makefile @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_asm330lhhx-y := st_asm330lhhx_core.o st_asm330lhhx_buffer.o \ + st_asm330lhhx_events.o \ + st_asm330lhhx_shub.o \ + st_asm330lhhx_mlc.o + +st_asm330lhhx-$(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) += st_asm330lhhx_hwtimestamp.o + +obj-$(CONFIG_IIO_ST_ASM330LHHX) += st_asm330lhhx.o +obj-$(CONFIG_IIO_ST_ASM330LHHX_I2C) += st_asm330lhhx_i2c.o +obj-$(CONFIG_IIO_ST_ASM330LHHX_SPI) += st_asm330lhhx_spi.o +obj-$(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) += st_asm330lhhx_hwtimestamp.o diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx.h b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx.h new file mode 100644 index 000000000000..3b57fde4d202 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx.h @@ -0,0 +1,1013 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_asm330lhhx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2019 STMicroelectronics Inc. + */ + +#ifndef ST_ASM330LHHX_H +#define ST_ASM330LHHX_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#define ST_ASM330LHHX_DEBUG_DISCHARGE + +#define ST_ASM330LHHX_MAX_ODR 833 +#define ST_ASM330LHHX_ODR_LIST_SIZE 8 +#define ST_ASM330LHHX_ODR_EXPAND(odr, uodr) ((odr * 1000000) + uodr) + +#define ST_ASM330LHH_DEV_NAME "asm330lhh" +#define ST_ASM330LHHX_DEV_NAME "asm330lhhx" + +#define ST_ASM330LHHX_DEFAULT_XL_FS_INDEX 2 +#define ST_ASM330LHHX_DEFAULT_XL_ODR_INDEX 1 +#define ST_ASM330LHHX_DEFAULT_G_FS_INDEX 3 +#define ST_ASM330LHHX_DEFAULT_G_ODR_INDEX 1 +#define ST_ASM330LHHX_DEFAULT_T_FS_INDEX 0 +#define ST_ASM330LHHX_DEFAULT_T_ODR_INDEX 1 + +#define ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_ASM330LHHX_REG_SHUB_REG_MASK BIT(6) +#define ST_ASM330LHHX_REG_FUNC_CFG_MASK BIT(7) +#define ST_ASM330LHHX_REG_ACCESS_MASK GENMASK(7, 6) + +#define ST_ASM330LHHX_REG_FIFO_CTRL1_ADDR 0x07 +#define ST_ASM330LHHX_REG_FIFO_CTRL2_ADDR 0x08 +#define ST_ASM330LHHX_REG_FIFO_WTM_MASK GENMASK(8, 0) +#define ST_ASM330LHHX_REG_FIFO_WTM8_MASK BIT(0) +#define ST_ASM330LHHX_REG_FIFO_STATUS_DIFF GENMASK(9, 0) + +#define ST_ASM330LHHX_REG_FIFO_CTRL3_ADDR 0x09 +#define ST_ASM330LHHX_REG_BDR_XL_MASK GENMASK(3, 0) +#define ST_ASM330LHHX_REG_BDR_GY_MASK GENMASK(7, 4) + +#define ST_ASM330LHHX_REG_FIFO_CTRL4_ADDR 0x0a +#define ST_ASM330LHHX_REG_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_ASM330LHHX_REG_DEC_TS_MASK GENMASK(7, 6) +#define ST_ASM330LHHX_REG_ODR_T_BATCH_MASK GENMASK(5, 4) + +#define ST_ASM330LHHX_REG_INT1_CTRL_ADDR 0x0d +#define ST_ASM330LHHX_REG_INT2_CTRL_ADDR 0x0e +#define ST_ASM330LHHX_REG_INT_FIFO_TH_MASK BIT(3) + +#define ST_ASM330LHHX_REG_WHOAMI_ADDR 0x0f +#define ST_ASM330LHHX_WHOAMI_VAL 0x6b + +#define ST_ASM330LHHX_CTRL1_XL_ADDR 0x10 +#define ST_ASM330LHHX_CTRL2_G_ADDR 0x11 + +#define ST_ASM330LHHX_REG_CTRL3_C_ADDR 0x12 +#define ST_ASM330LHHX_REG_SW_RESET_MASK BIT(0) +#define ST_ASM330LHHX_REG_PP_OD_MASK BIT(4) +#define ST_ASM330LHHX_REG_H_LACTIVE_MASK BIT(5) +#define ST_ASM330LHHX_REG_BDU_MASK BIT(6) +#define ST_ASM330LHHX_REG_BOOT_MASK BIT(7) + +#define ST_ASM330LHHX_REG_CTRL4_C_ADDR 0x13 +#define ST_ASM330LHHX_REG_DRDY_MASK BIT(3) + +#define ST_ASM330LHHX_REG_CTRL5_C_ADDR 0x14 +#define ST_ASM330LHHX_REG_ROUNDING_MASK GENMASK(6, 5) +#define ST_ASM330LHHX_REG_ST_G_MASK GENMASK(3, 2) +#define ST_ASM330LHHX_REG_ST_XL_MASK GENMASK(1, 0) +#define ST_ASM330LHHX_SELFTEST_ACCEL_MIN 737 +#define ST_ASM330LHHX_SELFTEST_ACCEL_MAX 13934 +#define ST_ASM330LHHX_SELFTEST_GYRO_MIN 2142 +#define ST_ASM330LHHX_SELFTEST_GYRO_MAX 10000 + +#define ST_ASM330LHHX_SELF_TEST_DISABLED_VAL 0 +#define ST_ASM330LHHX_SELF_TEST_POS_SIGN_VAL 1 +#define ST_ASM330LHHX_SELF_TEST_NEG_ACCEL_SIGN_VAL 2 +#define ST_ASM330LHHX_SELF_TEST_NEG_GYRO_SIGN_VAL 3 + +#define ST_ASM330LHHX_REG_CTRL6_C_ADDR 0x15 +#define ST_ASM330LHHX_REG_XL_HM_MODE_MASK BIT(4) + +#define ST_ASM330LHHX_REG_CTRL7_G_ADDR 0x16 +#define ST_ASM330LHHX_REG_G_HM_MODE_MASK BIT(7) + +#define ST_ASM330LHHX_REG_CTRL9_XL_ADDR 0x18 +#define ST_ASM330LHHX_REG_DEVICE_CONF_MASK BIT(1) + +#define ST_ASM330LHHX_REG_CTRL10_C_ADDR 0x19 +#define ST_ASM330LHHX_REG_TIMESTAMP_EN_MASK BIT(5) + +#define ST_ASM330LHHX_REG_STATUS_ADDR 0x1e +#define ST_ASM330LHHX_REG_STATUS_XLDA BIT(0) +#define ST_ASM330LHHX_REG_STATUS_GDA BIT(1) +#define ST_ASM330LHHX_REG_STATUS_TDA BIT(2) + +#define ST_ASM330LHHX_REG_OUT_TEMP_L_ADDR 0x20 + +#define ST_ASM330LHHX_REG_OUTX_L_A_ADDR 0x28 +#define ST_ASM330LHHX_REG_OUTY_L_A_ADDR 0x2a +#define ST_ASM330LHHX_REG_OUTZ_L_A_ADDR 0x2c + +#define ST_ASM330LHHX_REG_OUTX_L_G_ADDR 0x22 +#define ST_ASM330LHHX_REG_OUTY_L_G_ADDR 0x24 +#define ST_ASM330LHHX_REG_OUTZ_L_G_ADDR 0x26 + +#define ST_ASM330LHHX_FSM_STATUS_A_MAINPAGE 0x36 +#define ST_ASM330LHHX_FSM_STATUS_B_MAINPAGE 0x37 +#define ST_ASM330LHHX_MLC_STATUS_MAINPAGE 0x38 + +#define ST_ASM330LHHX_REG_TIMESTAMP0_ADDR 0x40 + +#define ST_ASM330LHHX_REG_TAP_CFG0_ADDR 0x56 +#define ST_ASM330LHHX_REG_LIR_MASK BIT(0) + +#define ST_ASM330LHHX_REG_THS_6D_ADDR 0x59 +#define ST_ASM330LHHX_SIXD_THS_MASK GENMASK(6, 5) + +#define ST_ASM330LHHX_REG_WAKE_UP_THS_ADDR 0x5b +#define ST_ASM330LHHX_WAKE_UP_THS_MASK GENMASK(5, 0) + +#define ST_ASM330LHHX_REG_WAKE_UP_DUR_ADDR 0x5c +#define ST_ASM330LHHX_WAKE_UP_DUR_MASK GENMASK(6, 5) + +#define ST_ASM330LHHX_REG_FREE_FALL_ADDR 0x5d +#define ST_ASM330LHHX_FF_THS_MASK GENMASK(2, 0) + +#define ST_ASM330LHHX_REG_MD1_CFG_ADDR 0x5e +#define ST_ASM330LHHX_REG_MD2_CFG_ADDR 0x5f +#define ST_ASM330LHHX_REG_INT2_TIMESTAMP_MASK BIT(0) +#define ST_ASM330LHHX_REG_INT_EMB_FUNC_MASK BIT(1) + +#define ST_ASM330LHHX_INTERNAL_FREQ_FINE 0x63 + +/* shub registers */ +#define ST_ASM330LHHX_REG_MASTER_CONFIG_ADDR 0x14 +#define ST_ASM330LHHX_REG_WRITE_ONCE_MASK BIT(6) +#define ST_ASM330LHHX_REG_SHUB_PU_EN_MASK BIT(3) +#define ST_ASM330LHHX_REG_MASTER_ON_MASK BIT(2) + +#define ST_ASM330LHHX_REG_SLV0_ADDR 0x15 +#define ST_ASM330LHHX_REG_SLV0_CFG 0x17 +#define ST_ASM330LHHX_REG_SLV1_ADDR 0x18 +#define ST_ASM330LHHX_REG_SLV2_ADDR 0x1b +#define ST_ASM330LHHX_REG_SLV3_ADDR 0x1e +#define ST_ASM330LHHX_REG_DATAWRITE_SLV0_ADDR 0x21 +#define ST_ASM330LHHX_REG_BATCH_EXT_SENS_EN_MASK BIT(3) +#define ST_ASM330LHHX_REG_SLAVE_NUMOP_MASK GENMASK(2, 0) + +#define ST_ASM330LHHX_REG_STATUS_MASTER_ADDR 0x22 +#define ST_ASM330LHHX_REG_SENS_HUB_ENDOP_MASK BIT(0) + +#define ST_ASM330LHHX_REG_SLV0_OUT_ADDR 0x02 + +/* embedded function registers */ +#define ST_ASM330LHHX_PAGE_SEL_ADDR 0x02 +#define ST_ASM330LHHX_PAGE_SEL_MASK GENMASK(7, 4) + +#define ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_ASM330LHHX_FSM_EN_MASK BIT(0) +#define ST_ASM330LHHX_MLC_EN_MASK BIT(4) + +#define ST_ASM330LHHX_FSM_INT1_A_ADDR 0x0b +#define ST_ASM330LHHX_FSM_INT1_B_ADDR 0x0c +#define ST_ASM330LHHX_MLC_INT1_ADDR 0x0d + +#define ST_ASM330LHHX_FSM_INT2_A_ADDR 0x0f +#define ST_ASM330LHHX_FSM_INT2_B_ADDR 0x10 +#define ST_ASM330LHHX_MLC_INT2_ADDR 0x11 + +#define ST_ASM330LHHX_REG_MLC_STATUS_ADDR 0x15 + +#define ST_ASM330LHHX_REG_PAGE_RW 0x17 +#define ST_ASM330LHHX_REG_EMB_FUNC_LIR_MASK BIT(7) +#define ST_ASM330LHHX_REG_PAGE_WRITE_MASK BIT(6) +#define ST_ASM330LHHX_REG_PAGE_READ_MASK BIT(5) + +#define ST_ASM330LHHX_FSM_ENABLE_A_ADDR 0x46 +#define ST_ASM330LHHX_FSM_ENABLE_B_ADDR 0x47 + +#define ST_ASM330LHHX_FSM_OUTS1_ADDR 0x4c + +#define ST_ASM330LHHX_REG_EMB_FUNC_INIT_B_ADDR 0x67 +#define ST_ASM330LHHX_FSM_INIT_MASK BIT(0) +#define ST_ASM330LHHX_MLC_INIT_MASK BIT(4) + +#define ST_ASM330LHHX_REG_MLC0_SRC_ADDR 0x70 + +/* Timestamp Tick 25us/LSB */ +#define ST_ASM330LHHX_TS_DELTA_NS 25000ULL + +/* Temperature in uC */ +#define ST_ASM330LHHX_TEMP_GAIN 256 +#define ST_ASM330LHHX_TEMP_FS_GAIN 1000000 / ST_ASM330LHHX_TEMP_GAIN +#define ST_ASM330LHHX_TEMP_OFFSET 6400 + +/* FIFO simple size and depth */ +#define ST_ASM330LHHX_SAMPLE_SIZE 6 +#define ST_ASM330LHHX_TS_SAMPLE_SIZE 4 +#define ST_ASM330LHHX_TAG_SIZE 1 +#define ST_ASM330LHHX_FIFO_SAMPLE_SIZE (ST_ASM330LHHX_SAMPLE_SIZE + \ + ST_ASM330LHHX_TAG_SIZE) +#define ST_ASM330LHHX_MAX_FIFO_DEPTH 416 + +#define ST_ASM330LHHX_DEFAULT_KTIME (200000000) +#define ST_ASM330LHHX_FAST_KTIME (5000000) + +#define ST_ASM330LHHX_DATA_CHANNEL(chan_type, addr, mod, ch2, scan_idx, \ + rb, sb, sg, ex_info) \ +{ \ + .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 = ex_info, \ +} + +static const struct iio_event_spec st_asm330lhhx_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_asm330lhhx_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_ASM330LHHX_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_asm330lhhx_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_ASM330LHHX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +enum st_asm330lhhx_fsm_mlc_enable_id { + ST_ASM330LHHX_MLC_FSM_DISABLED = 0, + ST_ASM330LHHX_MLC_ENABLED = BIT(0), + ST_ASM330LHHX_FSM_ENABLED = BIT(1), +}; + +/** + * struct mlc_config_t - + * @mlc_int_mask: interrupt register mask + * @fsm_enabled_mask: enable fsm register mask + * @fsm_int_mask: interrupt register mask + * @mlc_configured: number of mlc configured + * @fsm_configured: number of fsm configured + * @bin_len: fw binary size + * @mlc_int_pin: where route mlc int pin + * @mlc_fsm_en: mlc and fsm enable bit reported by ucf file + * @fsm_mlc_requested_odr, fsm_mlc_requested_uodr: Min required ODR by + * MLC/FSM. + * @status: mlc/fsm status + */ +struct st_asm330lhhx_mlc_config_t { + u8 mlc_int_mask; + u8 fsm_enabled_mask[2]; + u8 fsm_int_mask[2]; + u8 mlc_configured; + u8 fsm_configured; + u16 bin_len; + int mlc_int_pin; + u8 mlc_fsm_en; + u16 fsm_mlc_requested_odr; + u32 fsm_mlc_requested_uodr; + enum st_asm330lhhx_fsm_mlc_enable_id status; +}; + +/** + * struct st_asm330lhhx_reg - Generic sensor register description (addr + mask) + * @addr: Address of register. + * @mask: Bitmask register for proper usage. + */ +struct st_asm330lhhx_reg { + u8 addr; + u8 mask; +}; + +enum st_asm330lhhx_suspend_resume_register { + ST_ASM330LHHX_CTRL1_XL_REG = 0, + ST_ASM330LHHX_CTRL2_G_REG, + ST_ASM330LHHX_REG_CTRL3_C_REG, + ST_ASM330LHHX_REG_CTRL4_C_REG, + ST_ASM330LHHX_REG_CTRL5_C_REG, + ST_ASM330LHHX_REG_CTRL10_C_REG, + ST_ASM330LHHX_REG_TAP_CFG0_REG, + ST_ASM330LHHX_REG_INT1_CTRL_REG, + ST_ASM330LHHX_REG_INT2_CTRL_REG, + ST_ASM330LHHX_REG_FIFO_CTRL1_REG, + ST_ASM330LHHX_REG_FIFO_CTRL2_REG, + ST_ASM330LHHX_REG_FIFO_CTRL3_REG, + ST_ASM330LHHX_REG_FIFO_CTRL4_REG, + ST_ASM330LHHX_REG_EMB_FUNC_EN_B_REG, + ST_ASM330LHHX_REG_FSM_INT1_A_REG, + ST_ASM330LHHX_REG_FSM_INT1_B_REG, + ST_ASM330LHHX_REG_MLC_INT1_REG, + ST_ASM330LHHX_REG_FSM_INT2_A_REG, + ST_ASM330LHHX_REG_FSM_INT2_B_REG, + ST_ASM330LHHX_REG_MLC_INT2_REG, + ST_ASM330LHHX_SUSPEND_RESUME_REGS, +}; + +/** + * Define embedded functions register access + * + * FUNC_CFG_ACCESS_0 is default bank + * FUNC_CFG_ACCESS_SHUB_REG Enable access to the sensor hub (I2C master) + * registers. + * FUNC_CFG_ACCESS_FUNC_CFG Enable access to the embedded functions + * configuration registers. + */ +enum st_asm330lhh_page_sel_register { + FUNC_CFG_ACCESS_0 = 0, + FUNC_CFG_ACCESS_SHUB_REG, + FUNC_CFG_ACCESS_FUNC_CFG, +}; + +/** + * struct st_asm330lhhx_suspend_resume_entry - Register value for + * backup/restore + * + * @page: Page bank reg map. + * @addr: Address of register. + * @val: Register value. + * @mask: Bitmask register for proper usage. + */ +struct st_asm330lhhx_suspend_resume_entry { + u8 page; + u8 addr; + u8 val; + u8 mask; +}; + +/** + * struct st_asm330lhhx_odr - Single ODR entry + * @hz: Most significant part of the sensor ODR (Hz). + * @uhz: Less significant part of the sensor ODR (micro Hz). + * @val: ODR register value. + * @batch_val: Batching ODR register value. + */ +struct st_asm330lhhx_odr { + u16 hz; + u32 uhz; + u8 val; + u8 batch_val; +}; + +/** + * struct st_asm330lhhx_odr_table_entry - Sensor ODR table + * @size: Size of ODR table. + * @reg: ODR register. + * @pm: Power mode register. + * @batching_reg: ODR register for batching on fifo. + * @odr_avl: Array of supported ODR value. + */ +struct st_asm330lhhx_odr_table_entry { + u8 size; + struct st_asm330lhhx_reg reg; + struct st_asm330lhhx_reg pm; + struct st_asm330lhhx_reg batching_reg; + struct st_asm330lhhx_odr odr_avl[ST_ASM330LHHX_ODR_LIST_SIZE]; +}; + +/** + * struct st_asm330lhhx_fs - Full Scale sensor table entry + * @reg: Register description for FS settings. + * @gain: Sensor sensitivity (mdps/LSB, mg/LSB and uC/LSB). + * @val: FS register value. + */ +struct st_asm330lhhx_fs { + struct st_asm330lhhx_reg reg; + u32 gain; + u8 val; +}; + +#define ST_ASM330LHHX_FS_LIST_SIZE 6 +#define ST_ASM330LHHX_FS_ACC_LIST_SIZE 4 +#define ST_ASM330LHHX_FS_GYRO_LIST_SIZE 6 +#define ST_ASM330LHHX_FS_TEMP_LIST_SIZE 1 + +/** + * struct st_asm330lhhx_fs_table_entry - Full Scale sensor table + * @size: Full Scale sensor table size. + * @fs_avl: Full Scale list entries. + */ +struct st_asm330lhhx_fs_table_entry { + u8 size; + struct st_asm330lhhx_fs fs_avl[ST_ASM330LHHX_FS_LIST_SIZE]; +}; + +#define ST_ASM330LHHX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_ASM330LHHX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_ASM330LHHX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_ASM330LHHX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488000) + +#define ST_ASM330LHHX_GYRO_FS_125_GAIN IIO_DEGREE_TO_RAD(4370000) +#define ST_ASM330LHHX_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_ASM330LHHX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_ASM330LHHX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_ASM330LHHX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_ASM330LHHX_GYRO_FS_4000_GAIN IIO_DEGREE_TO_RAD(140000000) + +enum st_asm330lhhx_sensor_id { + ST_ASM330LHHX_ID_GYRO = 0, + ST_ASM330LHHX_ID_ACC, + ST_ASM330LHHX_ID_TEMP, + ST_ASM330LHHX_ID_EXT0, + ST_ASM330LHHX_ID_EXT1, + ST_ASM330LHHX_ID_MLC, + ST_ASM330LHHX_ID_MLC_0, + ST_ASM330LHHX_ID_MLC_1, + ST_ASM330LHHX_ID_MLC_2, + ST_ASM330LHHX_ID_MLC_3, + ST_ASM330LHHX_ID_MLC_4, + ST_ASM330LHHX_ID_MLC_5, + ST_ASM330LHHX_ID_MLC_6, + ST_ASM330LHHX_ID_MLC_7, + ST_ASM330LHHX_ID_FSM_0, + ST_ASM330LHHX_ID_FSM_1, + ST_ASM330LHHX_ID_FSM_2, + ST_ASM330LHHX_ID_FSM_3, + ST_ASM330LHHX_ID_FSM_4, + ST_ASM330LHHX_ID_FSM_5, + ST_ASM330LHHX_ID_FSM_6, + ST_ASM330LHHX_ID_FSM_7, + ST_ASM330LHHX_ID_FSM_8, + ST_ASM330LHHX_ID_FSM_9, + ST_ASM330LHHX_ID_FSM_10, + ST_ASM330LHHX_ID_FSM_11, + ST_ASM330LHHX_ID_FSM_12, + ST_ASM330LHHX_ID_FSM_13, + ST_ASM330LHHX_ID_FSM_14, + ST_ASM330LHHX_ID_FSM_15, + ST_ASM330LHHX_ID_EVENT, + ST_ASM330LHHX_ID_FF = ST_ASM330LHHX_ID_EVENT, + ST_ASM330LHHX_ID_SC, + ST_ASM330LHHX_ID_TRIGGER, + ST_ASM330LHHX_ID_WK = ST_ASM330LHHX_ID_TRIGGER, + ST_ASM330LHHX_ID_6D, + ST_ASM330LHHX_ID_MAX, +}; + +static const enum st_asm330lhhx_sensor_id st_asm330lhhx_mlc_sensor_list[] = { + [0] = ST_ASM330LHHX_ID_MLC_0, + [1] = ST_ASM330LHHX_ID_MLC_1, + [2] = ST_ASM330LHHX_ID_MLC_2, + [3] = ST_ASM330LHHX_ID_MLC_3, + [4] = ST_ASM330LHHX_ID_MLC_4, + [5] = ST_ASM330LHHX_ID_MLC_5, + [6] = ST_ASM330LHHX_ID_MLC_6, + [7] = ST_ASM330LHHX_ID_MLC_7, +}; + +static const enum st_asm330lhhx_sensor_id st_asm330lhhx_fsm_sensor_list[] = { + [0] = ST_ASM330LHHX_ID_FSM_0, + [1] = ST_ASM330LHHX_ID_FSM_1, + [2] = ST_ASM330LHHX_ID_FSM_2, + [3] = ST_ASM330LHHX_ID_FSM_3, + [4] = ST_ASM330LHHX_ID_FSM_4, + [5] = ST_ASM330LHHX_ID_FSM_5, + [6] = ST_ASM330LHHX_ID_FSM_6, + [7] = ST_ASM330LHHX_ID_FSM_7, + [8] = ST_ASM330LHHX_ID_FSM_8, + [9] = ST_ASM330LHHX_ID_FSM_9, + [10] = ST_ASM330LHHX_ID_FSM_10, + [11] = ST_ASM330LHHX_ID_FSM_11, + [12] = ST_ASM330LHHX_ID_FSM_12, + [13] = ST_ASM330LHHX_ID_FSM_13, + [14] = ST_ASM330LHHX_ID_FSM_14, + [15] = ST_ASM330LHHX_ID_FSM_15, +}; + +#define ST_ASM330LHHX_ID_ALL_FSM_MLC (BIT_ULL(ST_ASM330LHHX_ID_MLC_0) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_1) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_2) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_3) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_4) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_5) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_6) | \ + BIT_ULL(ST_ASM330LHHX_ID_MLC_7) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_0) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_1) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_2) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_3) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_4) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_5) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_6) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_7) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_8) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_9) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_10) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_11) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_12) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_13) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_14) | \ + BIT_ULL(ST_ASM330LHHX_ID_FSM_15)) + +/* + * HW devices that can wakeup the target + */ +#define ST_ASM330LHHX_WAKE_UP_SENSORS (BIT_ULL(ST_ASM330LHHX_ID_GYRO) | \ + BIT_ULL(ST_ASM330LHHX_ID_ACC) | \ + ST_ASM330LHHX_ID_ALL_FSM_MLC) + +/* this is the minimal ODR for wake-up sensors and dependencies */ +#define ST_ASM330LHHX_MIN_ODR_IN_WAKEUP 26 + +enum st_asm330lhhx_fifo_mode { + ST_ASM330LHHX_FIFO_BYPASS = 0x0, + ST_ASM330LHHX_FIFO_CONT = 0x6, +}; + +enum { + ST_ASM330LHHX_HW_FLUSH, + ST_ASM330LHHX_HW_OPERATIONAL, +}; + +struct st_asm330lhhx_ext_dev_info { + const struct st_asm330lhhx_ext_dev_settings *ext_dev_settings; + u8 ext_dev_i2c_addr; +}; + +enum st_asm330lhhx_hw_id { + ST_ASM330LHH_ID, + ST_ASM330LHHX_ID, + ST_ASM330LHHX_MAX_ID, +}; + +enum st_asm330lhhx_pm_t { + ST_ASM330LHHX_HP_MODE = 0, + ST_ASM330LHHX_LP_MODE, + ST_ASM330LHHX_NO_MODE, +}; + +/** + * struct st_asm330lhhx_pm_table - Power mode table + * + * @mode: Power mode string. + * @pm: Power mode setting. + */ +struct st_asm330lhhx_pm_table { + char *mode; + enum st_asm330lhhx_pm_t pm; +}; + +/** + * struct st_asm330lhhx_settings - ST IMU sensor settings + * + * @hw_id: Hw id supported by the driver configuration. + * @name: Device name supported by the driver configuration. + * @st_mlc_probe: MLC probe flag. + * @st_shub_probe: SHUB probe flag. + * @st_power_mode: Support power mode flag. + */ +struct st_asm330lhhx_settings { + struct { + enum st_asm330lhhx_hw_id hw_id; + const char *name; + } id; + bool st_mlc_probe; + bool st_shub_probe; + bool st_power_mode; +}; + +/** + * struct st_asm330lhhx_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_asm330lhhx_hw. + * @ext_dev_info: For sensor hub indicate device info struct. + * @trig: Trigger used by IIO event sensors. + * @gain: Configured sensor sensitivity. + * @offset: Sensor data offset. + * @decimator: Sensor decimator + * @dec_counter: Sensor decimator counter + * @odr: Output data rate of the sensor [Hz]. + * @uodr: Output data rate of the sensor [uHz]. + * @discharged_samples: Report number of samples discharded by drdy mask + * filters. + * @max_watermark: Max supported watermark level. + * @watermark: Sensor watermark level. + * @pm: sensor power mode (HP, LP). + * @last_fifo_timestamp: Store last sample timestamp in FIFO, used by flush + * @selftest_status: Last status of self test output + * @min_st, @max_st: Min/Max acc/gyro data values during self test procedure + * @status_reg: Generic status register used by MLC/FSM. + * @outreg_addr: MLC/FSM output data registers. + * @status: MLC/FSM enable status. + * @conf: Used in case of sensor event to manage configuration. + * @scan: Scan buffer for triggered sensors event. + */ +struct st_asm330lhhx_sensor { + char name[32]; + enum st_asm330lhhx_sensor_id id; + struct st_asm330lhhx_hw *hw; + struct st_asm330lhhx_ext_dev_info ext_dev_info; + struct iio_trigger *trig; + + union { + /* sensor with odrs, gain and offset */ + struct { + u32 gain; + u32 offset; + u8 decimator; + u8 dec_counter; + int odr; + int uodr; + +#ifdef ST_ASM330LHHX_DEBUG_DISCHARGE + u32 discharged_samples; +#endif /* ST_ASM330LHHX_DEBUG_DISCHARGE */ + + u16 max_watermark; + u16 watermark; + enum st_asm330lhhx_pm_t pm; + s64 last_fifo_timestamp; + + /* self test */ + int8_t selftest_status; + int min_st; + int max_st; + + /* mlc / fsm registers */ + u8 status_reg; + u8 outreg_addr; + enum st_asm330lhhx_fsm_mlc_enable_id status; + }; + + /* sensor specific data configuration */ + struct { + u32 conf[6]; + + /* ensure natural alignment of timestamp */ + struct { + u8 event; + s64 ts __aligned(8); + } scan; + }; + }; +}; + +/** + * struct st_asm330lhhx_hw - ST IMU MEMS hw instance + * @dev: Pointer to instance of struct device (I2C or SPI). + * @irq: Device interrupt line (I2C or SPI). + * @regmap: Register map of the device. + * @int_pin: Save interrupt pin used by sensor. + * @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. + * @ext_data_len: SHUB external sensor data len. + * @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. + * @ts_offset: Hw timestamp offset. + * @ts_delta_ns: Calibrate delta time tick. + * @hw_ts: Latest hw timestamp from the sensor. + * @val_ts_old: Hold hw timestamp for timer rollover. + * @hw_ts_high: Save MSB hw timestamp. + * @tsample: Timestamp for each sensor sample. + * @delta_ts: Delta time between two consecutive interrupts. + * @ts: Latest timestamp from irq handler. + * @i2c_master_pu: I2C master line Pull Up configuration. + * @module_id: identify iio devices of the same sensor module. + * @odr_table_entry: Sensors ODR table. + * @iio_devs: Pointers to acc/gyro iio_dev instances. + * @vdd_supply: Voltage regulator for VDD. + * @vddio_supply: Voltage regulator for VDDIIO. + * @orientation: sensor chip orientation relative to main hardware. + * @settings: ST IMU sensor settings. + * @mlc_config: Pointer to MLC/FSM configuration structure. + * @preload_mlc: Indicate to preload firmware for MLC/FSM. + */ +struct st_asm330lhhx_hw { + struct device *dev; + int irq; + struct regmap *regmap; + int int_pin; + + struct mutex lock; + struct mutex fifo_lock; + struct mutex page_lock; + + enum st_asm330lhhx_fifo_mode fifo_mode; + unsigned long state; + u64 enable_mask; + u64 requested_mask; + u8 ext_data_len; + s64 hw_timestamp_global; + +#if defined (CONFIG_IIO_ST_ASM330LHHX_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_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + + 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; + u8 i2c_master_pu; + u32 module_id; + + const struct st_asm330lhhx_odr_table_entry *odr_table_entry; + struct iio_dev *iio_devs[ST_ASM330LHHX_ID_MAX]; + + struct regulator *vdd_supply; + struct regulator *vddio_supply; + + struct iio_mount_matrix orientation; + + const struct st_asm330lhhx_settings *settings; + + struct st_asm330lhhx_mlc_config_t *mlc_config; + bool preload_mlc; +}; + +/** + * struct st_asm330lhhx_ff_th - Free Fall threshold table + * @mg: Threshold in mg. + * @val: Register value. + */ +struct st_asm330lhhx_ff_th { + u32 mg; + u8 val; +}; + +/** + * struct st_asm330lhhx_6D_th - 6D threshold table + * @deg: Threshold in degrees. + * @val: Register value. + */ +struct st_asm330lhhx_6D_th { + u8 deg; + u8 val; +}; + +extern const struct dev_pm_ops st_asm330lhhx_pm_ops; + +static inline int __st_asm330lhhx_write_with_mask(struct st_asm330lhhx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_ASM330LHHX_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int +st_asm330lhhx_update_bits_locked(struct st_asm330lhhx_hw *hw, unsigned int addr, + unsigned int mask, unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_asm330lhhx_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +/* use when mask is constant */ +static inline int +st_asm330lhhx_write_with_mask_locked(struct st_asm330lhhx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_asm330lhhx_write_with_mask(hw, addr, mask, data); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_asm330lhhx_read_locked(struct st_asm330lhhx_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_asm330lhhx_write_locked(struct st_asm330lhhx_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_asm330lhhx_set_page_access(struct st_asm330lhhx_hw *hw, + unsigned int val, + unsigned int mask) +{ + return regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR, + mask, + ST_ASM330LHHX_SHIFT_VAL(val, mask)); +} + +static inline bool +st_asm330lhhx_is_fifo_enabled(struct st_asm330lhhx_hw *hw) +{ + return hw->enable_mask & (BIT_ULL(ST_ASM330LHHX_ID_GYRO) | + BIT_ULL(ST_ASM330LHHX_ID_ACC)); +} + +static inline s64 st_asm330lhhx_get_time_ns(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +static inline int +st_asm330lhhx_read_page_locked(struct st_asm330lhhx_hw *hw, + unsigned int addr, + void *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + err = regmap_bulk_read(hw->regmap, addr, val, len); + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_asm330lhhx_write_page_locked(struct st_asm330lhhx_hw *hw, + unsigned int addr, + unsigned int *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + err = regmap_bulk_write(hw->regmap, addr, val, len); + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_asm330lhhx_update_page_bits_locked(struct st_asm330lhhx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + err = regmap_update_bits(hw->regmap, addr, mask, val); + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline +bool st_asm330lhhx_fsm_running(struct st_asm330lhhx_hw *hw) +{ + return hw->enable_mask & + GENMASK_ULL(ST_ASM330LHHX_ID_FSM_15, + ST_ASM330LHHX_ID_FSM_0); +} + +static inline +bool st_asm330lhhx_mlc_running(struct st_asm330lhhx_hw *hw) +{ + return hw->enable_mask & + GENMASK_ULL(ST_ASM330LHHX_ID_MLC_7, + ST_ASM330LHHX_ID_MLC_0); +} + +int st_asm330lhhx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap); +int st_asm330lhhx_sensor_set_enable(struct st_asm330lhhx_sensor *sensor, + bool enable); +int st_asm330lhhx_buffers_setup(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_get_odr_from_reg(enum st_asm330lhhx_sensor_id id, + u8 reg_val, u16 *podr, u32 *puodr); +int st_asm330lhhx_get_batch_val(struct st_asm330lhhx_sensor *sensor, + int odr, int uodr, u8 *val); +int st_asm330lhhx_update_watermark(struct st_asm330lhhx_sensor *sensor, + u16 watermark); +ssize_t st_asm330lhhx_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_asm330lhhx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_asm330lhhx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_asm330lhhx_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_asm330lhhx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +int st_asm330lhhx_suspend_fifo(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_set_fifo_mode(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_fifo_mode fifo_mode); +int __st_asm330lhhx_set_sensor_batching_odr(struct st_asm330lhhx_sensor *sensor, + bool enable); +int st_asm330lhhx_update_batching(struct iio_dev *iio_dev, bool enable); +int st_asm330lhhx_reset_hwts(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_shub_probe(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_shub_set_enable(struct st_asm330lhhx_sensor *sensor, + bool enable); +int st_asm330lhhx_of_get_pin(struct st_asm330lhhx_hw *hw, int *pin); + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES +int st_asm330lhhx_event_handler(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_probe_event(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_set_wake_up_thershold(struct st_asm330lhhx_hw *hw, + int th_ug); +int st_asm330lhhx_set_wake_up_duration(struct st_asm330lhhx_hw *hw, + int dur_ms); +int st_asm330lhhx_set_freefall_threshold(struct st_asm330lhhx_hw *hw, + int th_mg); +int st_asm330lhhx_set_6D_threshold(struct st_asm330lhhx_hw *hw, + int deg); +int st_asm330lhhx_read_with_mask(struct st_asm330lhhx_hw *hw, u8 addr, + u8 mask, u8 *val); +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ + +#if defined (CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) +int st_asm330lhhx_hwtimesync_init(struct st_asm330lhhx_hw *hw); +#else /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ +static inline int +st_asm330lhhx_hwtimesync_init(struct st_asm330lhhx_hw *hw) +{ + return 0; +} +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + +int st_asm330lhhx_mlc_probe(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_mlc_remove(struct device *dev); +int st_asm330lhhx_mlc_check_status(struct st_asm330lhhx_hw *hw); +int st_asm330lhhx_mlc_init_preload(struct st_asm330lhhx_hw *hw); + +#endif /* ST_ASM330LHHX_H */ diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_buffer.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_buffer.c new file mode 100644 index 000000000000..79727c37f283 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_buffer.c @@ -0,0 +1,680 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2019 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_asm330lhhx.h" + +#define ST_ASM330LHHX_REG_FIFO_STATUS1_ADDR 0x3a +#define ST_ASM330LHHX_REG_TIMESTAMP2_ADDR 0x42 +#define ST_ASM330LHHX_REG_FIFO_DATA_OUT_TAG_ADDR 0x78 + +#define ST_ASM330LHHX_SAMPLE_DISCHARD 0x7ffd + +/* Timestamp convergence filter parameter */ +#define ST_ASM330LHHX_EWMA_LEVEL 120 +#define ST_ASM330LHHX_EWMA_DIV 128 + +#define ST_ASM330LHHX_TIMESTAMP_RESET_VALUE 0xaa + +enum { + ST_ASM330LHHX_GYRO_TAG = 0x01, + ST_ASM330LHHX_ACC_TAG = 0x02, + ST_ASM330LHHX_TEMP_TAG = 0x03, + ST_ASM330LHHX_TS_TAG = 0x04, + ST_ASM330LHHX_EXT0_TAG = 0x0f, + ST_ASM330LHHX_EXT1_TAG = 0x10, +}; + +/* Default timeout before to re-enable gyro */ +int delay_gyro = 10; +module_param(delay_gyro, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); +MODULE_PARM_DESC(delay_gyro, "Delay for Gyro arming"); +static bool delayed_enable_gyro; + +static inline s64 st_asm330lhhx_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_ASM330LHHX_EWMA_DIV - weight) * diff, + ST_ASM330LHHX_EWMA_DIV); + + return old + incr; +} + +inline int st_asm330lhhx_reset_hwts(struct st_asm330lhhx_hw *hw) +{ + u8 data = ST_ASM330LHHX_TIMESTAMP_RESET_VALUE; + int ret; + + ret = st_asm330lhhx_write_locked(hw, ST_ASM330LHHX_REG_TIMESTAMP2_ADDR, + data); + if (ret < 0) + return ret; + +#if defined(CONFIG_IIO_ST_ASM330LHHX_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_ASM330LHHX_FAST_KTIME); +#else /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + hw->hw_timestamp_global = (hw->hw_timestamp_global + (1LL << 32)) & + GENMASK_ULL(63, 32); +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + + hw->ts = st_asm330lhhx_get_time_ns(hw->iio_devs[0]); + hw->ts_offset = hw->ts; + hw->val_ts_old = 0; + hw->hw_ts_high = 0; + hw->tsample = 0ull; + + return 0; +} + +int st_asm330lhhx_set_fifo_mode(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_fifo_mode fifo_mode) +{ + int err; + + err = st_asm330lhhx_write_with_mask_locked(hw, + ST_ASM330LHHX_REG_FIFO_CTRL4_ADDR, + ST_ASM330LHHX_REG_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + if (fifo_mode == ST_ASM330LHHX_FIFO_BYPASS) + clear_bit(ST_ASM330LHHX_HW_OPERATIONAL, &hw->state); + else + set_bit(ST_ASM330LHHX_HW_OPERATIONAL, &hw->state); + + return 0; +} + +static inline int +st_asm330lhhx_set_sensor_batching_odr(struct st_asm330lhhx_sensor *s, + bool enable) +{ + enum st_asm330lhhx_sensor_id id = s->id; + struct st_asm330lhhx_hw *hw = s->hw; + u8 data = 0; + int err; + + if (enable) { + err = st_asm330lhhx_get_batch_val(s, s->odr, s->uodr, &data); + if (err < 0) + return err; + } + + return st_asm330lhhx_update_bits_locked(hw, + hw->odr_table_entry[id].batching_reg.addr, + hw->odr_table_entry[id].batching_reg.mask, + data); +} + +int st_asm330lhhx_update_watermark(struct st_asm330lhhx_sensor *sensor, + u16 watermark) +{ + u16 fifo_watermark = ST_ASM330LHHX_MAX_FIFO_DEPTH, cur_watermark = 0; + struct st_asm330lhhx_hw *hw = sensor->hw; + struct st_asm330lhhx_sensor *cur_sensor; + __le16 wdata; + int data = 0; + int i, err; + + for (i = ST_ASM330LHHX_ID_GYRO; i <= ST_ASM330LHHX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + + if (!(hw->enable_mask & BIT_ULL(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_ASM330LHHX_REG_FIFO_CTRL1_ADDR + 1, + &data); + if (err < 0) + goto out; + + fifo_watermark = ((data << 8) & ~ST_ASM330LHHX_REG_FIFO_WTM_MASK) | + (fifo_watermark & ST_ASM330LHHX_REG_FIFO_WTM_MASK); + wdata = cpu_to_le16(fifo_watermark); + + err = regmap_bulk_write(hw->regmap, + ST_ASM330LHHX_REG_FIFO_CTRL1_ADDR, + &wdata, sizeof(wdata)); +out: + mutex_unlock(&hw->page_lock); + + return err < 0 ? err : 0; +} + +static struct iio_dev *st_asm330lhhx_get_iiodev_from_tag(struct st_asm330lhhx_hw *hw, + u8 tag) +{ + struct iio_dev *iio_dev; + + switch (tag) { + case ST_ASM330LHHX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_GYRO]; + break; + case ST_ASM330LHHX_ACC_TAG: + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_ACC]; + break; + case ST_ASM330LHHX_TEMP_TAG: + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_TEMP]; + break; + case ST_ASM330LHHX_EXT0_TAG: + if (hw->enable_mask & BIT_ULL(ST_ASM330LHHX_ID_EXT0)) + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_EXT0]; + else + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_EXT1]; + break; + case ST_ASM330LHHX_EXT1_TAG: + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_EXT1]; + break; + default: + iio_dev = NULL; + break; + } + + return iio_dev; +} + +static inline void st_asm330lhhx_sync_hw_ts(struct st_asm330lhhx_hw *hw, s64 ts) +{ + s64 delta = ts - hw->hw_ts; + + hw->ts_offset = st_asm330lhhx_ewma(hw->ts_offset, delta, + ST_ASM330LHHX_EWMA_LEVEL); +} + +static int st_asm330lhhx_read_fifo(struct st_asm330lhhx_hw *hw) +{ + u8 iio_buf[ALIGN(ST_ASM330LHHX_SAMPLE_SIZE, sizeof(s64)) + + sizeof(s64) + sizeof(s64)]; + u8 buf[6 * ST_ASM330LHHX_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + __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; + + /* return if FIFO is already disabled */ + if (!test_bit(ST_ASM330LHHX_HW_OPERATIONAL, &hw->state)) { + dev_warn(hw->dev, "%s: FIFO in bypass mode\n", __func__); + + return 0; + } + + ts_irq = hw->ts - hw->delta_ts; + + err = st_asm330lhhx_read_locked(hw, ST_ASM330LHHX_REG_FIFO_STATUS1_ADDR, + &fifo_status, sizeof(fifo_status)); + if (err < 0) + return err; + + fifo_depth = le16_to_cpu(fifo_status) & ST_ASM330LHHX_REG_FIFO_STATUS_DIFF; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_ASM330LHHX_FIFO_SAMPLE_SIZE; + read_len = 0; + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_asm330lhhx_read_locked(hw, + ST_ASM330LHHX_REG_FIFO_DATA_OUT_TAG_ADDR, + buf, word_len); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_ASM330LHHX_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_ASM330LHHX_TAG_SIZE]; + tag = buf[i] >> 3; + + if (tag == ST_ASM330LHHX_TS_TAG) { + val = get_unaligned_le32(ptr); + +#if defined(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + hw->hw_timestamp_global = + (hw->hw_timestamp_global & + GENMASK_ULL(63, 32)) | + (u32)le32_to_cpu(get_unaligned_le32(ptr)); +#if defined(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + + if (hw->val_ts_old > val) + hw->hw_ts_high++; + + 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_asm330lhhx_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_ASM330LHHX_EWMA_LEVEL); + + if (!test_bit(ST_ASM330LHHX_HW_FLUSH, &hw->state)) + /* sync ap timestamp and sensor one */ + st_asm330lhhx_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 { + struct st_asm330lhhx_sensor *sensor; + + iio_dev = st_asm330lhhx_get_iiodev_from_tag(hw, tag); + if (!iio_dev) + continue; + + sensor = iio_priv(iio_dev); + + /* skip samples if not ready */ + drdymask = (s16)le16_to_cpu(get_unaligned_le16(ptr)); + if (unlikely(drdymask >= ST_ASM330LHHX_SAMPLE_DISCHARD)) { +#ifdef ST_ASM330LHHX_DEBUG_DISCHARGE + sensor->discharged_samples++; +#endif /* ST_ASM330LHHX_DEBUG_DISCHARGE */ + continue; + } + + memcpy(iio_buf, ptr, ST_ASM330LHHX_SAMPLE_SIZE); +#if defined(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + hw_timestamp_push = cpu_to_le64(hw->hw_timestamp_global); +#if defined(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + memcpy(&iio_buf[ALIGN(ST_ASM330LHHX_SAMPLE_SIZE, sizeof(s64))], + &hw_timestamp_push, sizeof(hw_timestamp_push)); + + hw->tsample = min_t(s64, + st_asm330lhhx_get_time_ns(hw->iio_devs[0]), + hw->tsample); + + /* support decimation for ODR < 12.5 Hz */ + if (sensor->dec_counter > 0) { + sensor->dec_counter--; + } else { + sensor->dec_counter = sensor->decimator; + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + sensor->last_fifo_timestamp = hw_timestamp_push; + } + } + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_asm330lhhx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +ssize_t st_asm330lhhx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +ssize_t st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_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_asm330lhhx_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_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + s64 event; + int count; + s64 type; + s64 fts; + s64 ts; + + mutex_lock(&hw->fifo_lock); + ts = st_asm330lhhx_get_time_ns(iio_dev); + hw->delta_ts = ts - hw->ts; + hw->ts = ts; + set_bit(ST_ASM330LHHX_HW_FLUSH, &hw->state); + count = st_asm330lhhx_read_fifo(hw); + sensor->dec_counter = 0; + fts = sensor->last_fifo_timestamp; + mutex_unlock(&hw->fifo_lock); + + type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fts); + + return size; +} + +int st_asm330lhhx_suspend_fifo(struct st_asm330lhhx_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + st_asm330lhhx_read_fifo(hw); + err = st_asm330lhhx_set_fifo_mode(hw, ST_ASM330LHHX_FIFO_BYPASS); + mutex_unlock(&hw->fifo_lock); + + return err; +} + +int st_asm330lhhx_update_batching(struct iio_dev *iio_dev, bool enable) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + + err = st_asm330lhhx_set_sensor_batching_odr(sensor, enable); + enable_irq(hw->irq); + + return err; +} + +static int st_asm330lhhx_update_fifo(struct iio_dev *iio_dev, + bool enable) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + int err; + + if (sensor->id == ST_ASM330LHHX_ID_GYRO && !enable) + delayed_enable_gyro = true; + + if (sensor->id == ST_ASM330LHHX_ID_GYRO && + enable && delayed_enable_gyro) { + delayed_enable_gyro = false; + msleep(delay_gyro); + } + + disable_irq(hw->irq); + +#if defined(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) + hrtimer_cancel(&hw->timesync_timer); + cancel_work_sync(&hw->timesync_work); +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + + if (sensor->id == ST_ASM330LHHX_ID_EXT0 || + sensor->id == ST_ASM330LHHX_ID_EXT1) { + err = st_asm330lhhx_shub_set_enable(sensor, enable); + if (err < 0) + goto out; + } else { + err = st_asm330lhhx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + + err = st_asm330lhhx_set_sensor_batching_odr(sensor, enable); + if (err < 0) + goto out; + } + + /* + * This is an auxiliary sensor, it need to get batched + * toghether at least with a primary sensor (Acc/Gyro). + */ + if (sensor->id == ST_ASM330LHHX_ID_TEMP) { + if (!(hw->enable_mask & (BIT_ULL(ST_ASM330LHHX_ID_ACC) | + BIT_ULL(ST_ASM330LHHX_ID_GYRO)))) { + struct st_asm330lhhx_sensor *acc_sensor; + u8 data = 0; + + acc_sensor = iio_priv(hw->iio_devs[ST_ASM330LHHX_ID_ACC]); + if (enable) { + err = st_asm330lhhx_get_batch_val(acc_sensor, + sensor->odr, sensor->uodr, + &data); + if (err < 0) + goto out; + } + + err = st_asm330lhhx_update_bits_locked(hw, + hw->odr_table_entry[ST_ASM330LHHX_ID_ACC].batching_reg.addr, + hw->odr_table_entry[ST_ASM330LHHX_ID_ACC].batching_reg.mask, + data); + if (err < 0) + goto out; + } + } + + err = st_asm330lhhx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (enable && hw->fifo_mode == ST_ASM330LHHX_FIFO_BYPASS) { + st_asm330lhhx_reset_hwts(hw); + err = st_asm330lhhx_set_fifo_mode(hw, ST_ASM330LHHX_FIFO_CONT); + } else if (!hw->enable_mask) { + err = st_asm330lhhx_set_fifo_mode(hw, ST_ASM330LHHX_FIFO_BYPASS); + } + +#if defined(CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP) + if (hw->fifo_mode != ST_ASM330LHHX_FIFO_BYPASS) { + hrtimer_start(&hw->timesync_timer, + ktime_set(0, 0), + HRTIMER_MODE_REL); + } +#endif /* CONFIG_IIO_ST_ASM330LHHX_ASYNC_HW_TIMESTAMP */ + +out: + enable_irq(hw->irq); + + return err; +} + +static irqreturn_t st_asm330lhhx_handler_irq(int irq, void *private) +{ + struct st_asm330lhhx_hw *hw = (struct st_asm330lhhx_hw *)private; + s64 ts = st_asm330lhhx_get_time_ns(hw->iio_devs[0]); + + hw->delta_ts = ts - hw->ts; + hw->ts = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_asm330lhhx_handler_thread(int irq, void *private) +{ + struct st_asm330lhhx_hw *hw = (struct st_asm330lhhx_hw *)private; + + if (hw->settings->st_mlc_probe) + st_asm330lhhx_mlc_check_status(hw); + + mutex_lock(&hw->fifo_lock); + st_asm330lhhx_read_fifo(hw); + clear_bit(ST_ASM330LHHX_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES + return st_asm330lhhx_event_handler(hw); +#else /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ + return IRQ_HANDLED; +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ +} + +static int st_asm330lhhx_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_asm330lhhx_update_fifo(iio_dev, true); +} + +static int st_asm330lhhx_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_asm330lhhx_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_asm330lhhx_fifo_ops = { + .preenable = st_asm330lhhx_fifo_preenable, + .postdisable = st_asm330lhhx_fifo_postdisable, +}; + +int st_asm330lhhx_buffers_setup(struct st_asm330lhhx_hw *hw) +{ +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + 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: + irq_active_low = false; + break; + case IRQF_TRIGGER_LOW: + irq_active_low = true; + break; + default: + dev_info(hw->dev, "mode %lx unsupported\n", irq_type); + return -EINVAL; + } + + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_CTRL3_C_ADDR, + ST_ASM330LHHX_REG_H_LACTIVE_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_H_LACTIVE_MASK, + irq_active_low)); + if (err < 0) + return err; + + if (device_property_read_bool(hw->dev, "drive-open-drain")) { + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_CTRL3_C_ADDR, + ST_ASM330LHHX_REG_PP_OD_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_PP_OD_MASK, + 1)); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_asm330lhhx_handler_irq, + st_asm330lhhx_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 = ST_ASM330LHHX_ID_GYRO; i <= ST_ASM330LHHX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + INDIO_BUFFER_SOFTWARE, + &st_asm330lhhx_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_asm330lhhx_fifo_ops; +#endif /* LINUX_VERSION_CODE */ + + } + + err = st_asm330lhhx_hwtimesync_init(hw); + if (err) + return err; + + return regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_FIFO_CTRL4_ADDR, + ST_ASM330LHHX_REG_DEC_TS_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_DEC_TS_MASK, + 1)); +} diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_core.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_core.c new file mode 100644 index 000000000000..513e3a46e0e8 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_core.c @@ -0,0 +1,2441 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2019 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_asm330lhhx.h" + +static struct st_asm330lhhx_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_asm330lhhx_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_ASM330LHHX_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_ASM330LHHX_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_ASM330LHHX_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_ASM330LHHX_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_ASM330LHHX_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_ASM330LHHX_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +static const struct st_asm330lhhx_power_mode_table { + char *string_mode; + enum st_asm330lhhx_pm_t val; +} st_asm330lhhx_power_mode[] = { + [0] = { + .string_mode = "HP_MODE", + .val = ST_ASM330LHHX_HP_MODE, + }, + [1] = { + .string_mode = "LP_MODE", + .val = ST_ASM330LHHX_LP_MODE, + }, +}; + +static struct st_asm330lhhx_suspend_resume_entry + st_asm330lhhx_suspend_resume[ST_ASM330LHHX_SUSPEND_RESUME_REGS] = { + [ST_ASM330LHHX_CTRL1_XL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_ASM330LHHX_CTRL2_G_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_ASM330LHHX_REG_CTRL3_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_CTRL3_C_ADDR, + .mask = ST_ASM330LHHX_REG_BDU_MASK | + ST_ASM330LHHX_REG_PP_OD_MASK | + ST_ASM330LHHX_REG_H_LACTIVE_MASK, + }, + [ST_ASM330LHHX_REG_CTRL4_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_CTRL4_C_ADDR, + .mask = ST_ASM330LHHX_REG_DRDY_MASK, + }, + [ST_ASM330LHHX_REG_CTRL5_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_CTRL5_C_ADDR, + .mask = ST_ASM330LHHX_REG_ROUNDING_MASK, + }, + [ST_ASM330LHHX_REG_CTRL10_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_CTRL10_C_ADDR, + .mask = ST_ASM330LHHX_REG_TIMESTAMP_EN_MASK, + }, + [ST_ASM330LHHX_REG_TAP_CFG0_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_TAP_CFG0_ADDR, + .mask = ST_ASM330LHHX_REG_LIR_MASK, + }, + [ST_ASM330LHHX_REG_INT1_CTRL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_INT1_CTRL_ADDR, + .mask = ST_ASM330LHHX_REG_INT_FIFO_TH_MASK, + }, + [ST_ASM330LHHX_REG_INT2_CTRL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_INT2_CTRL_ADDR, + .mask = ST_ASM330LHHX_REG_INT_FIFO_TH_MASK, + }, + [ST_ASM330LHHX_REG_FIFO_CTRL1_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_FIFO_CTRL1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ASM330LHHX_REG_FIFO_CTRL2_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_FIFO_CTRL2_ADDR, + .mask = ST_ASM330LHHX_REG_FIFO_WTM8_MASK, + }, + [ST_ASM330LHHX_REG_FIFO_CTRL3_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_FIFO_CTRL3_ADDR, + .mask = ST_ASM330LHHX_REG_BDR_XL_MASK | + ST_ASM330LHHX_REG_BDR_GY_MASK, + }, + [ST_ASM330LHHX_REG_FIFO_CTRL4_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_ASM330LHHX_REG_FIFO_CTRL4_ADDR, + .mask = ST_ASM330LHHX_REG_DEC_TS_MASK | + ST_ASM330LHHX_REG_ODR_T_BATCH_MASK, + }, + [ST_ASM330LHHX_REG_EMB_FUNC_EN_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + .mask = ST_ASM330LHHX_FSM_EN_MASK | + ST_ASM330LHHX_MLC_EN_MASK, + }, + [ST_ASM330LHHX_REG_FSM_INT1_A_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_FSM_INT1_A_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ASM330LHHX_REG_FSM_INT1_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_FSM_INT1_B_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ASM330LHHX_REG_MLC_INT1_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_MLC_INT1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ASM330LHHX_REG_FSM_INT2_A_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_FSM_INT2_A_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ASM330LHHX_REG_FSM_INT2_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_FSM_INT2_B_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ASM330LHHX_REG_MLC_INT2_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_ASM330LHHX_MLC_INT2_ADDR, + .mask = GENMASK(7, 0), + }, +}; + +static const struct st_asm330lhhx_odr_table_entry st_asm330lhhx_odr_table[] = { + [ST_ASM330LHHX_ID_ACC] = { + .size = 7, + .reg = { + .addr = ST_ASM330LHHX_CTRL1_XL_ADDR, + .mask = GENMASK(7, 4), + }, + .pm = { + .addr = ST_ASM330LHHX_REG_CTRL6_C_ADDR, + .mask = ST_ASM330LHHX_REG_XL_HM_MODE_MASK, + }, + .batching_reg = { + .addr = ST_ASM330LHHX_REG_FIFO_CTRL3_ADDR, + .mask = GENMASK(3, 0), + }, + .odr_avl[0] = { 12, 500000, 0x01, 0x01 }, + .odr_avl[1] = { 26, 0, 0x02, 0x02 }, + .odr_avl[2] = { 52, 0, 0x03, 0x03 }, + .odr_avl[3] = { 104, 0, 0x04, 0x04 }, + .odr_avl[4] = { 208, 0, 0x05, 0x05 }, + .odr_avl[5] = { 416, 0, 0x06, 0x06 }, + .odr_avl[6] = { 833, 0, 0x07, 0x07 }, + }, + [ST_ASM330LHHX_ID_GYRO] = { + .size = 7, + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(7, 4), + }, + .pm = { + .addr = ST_ASM330LHHX_REG_CTRL7_G_ADDR, + .mask = ST_ASM330LHHX_REG_G_HM_MODE_MASK, + }, + .batching_reg = { + .addr = ST_ASM330LHHX_REG_FIFO_CTRL3_ADDR, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12, 500000, 0x01, 0x01 }, + .odr_avl[1] = { 26, 0, 0x02, 0x02 }, + .odr_avl[2] = { 52, 0, 0x03, 0x03 }, + .odr_avl[3] = { 104, 0, 0x04, 0x04 }, + .odr_avl[4] = { 208, 0, 0x05, 0x05 }, + .odr_avl[5] = { 416, 0, 0x06, 0x06 }, + .odr_avl[6] = { 833, 0, 0x07, 0x07 }, + }, + [ST_ASM330LHHX_ID_TEMP] = { + .size = 2, + .batching_reg = { + .addr = ST_ASM330LHHX_REG_FIFO_CTRL4_ADDR, + .mask = GENMASK(5, 4), + }, + .odr_avl[0] = { 12, 500000, 0x02, 0x02 }, + .odr_avl[1] = { 52, 0, 0x03, 0x03 }, + }, +}; + +/** + * List of supported supported device settings + * + * The following table list all device features in terms of supported + * MLC and SHUB. + */ +static const struct st_asm330lhhx_settings st_asm330lhhx_sensor_settings[] = { + { + .id = { + .hw_id = ST_ASM330LHHX_ID, + .name = ST_ASM330LHHX_DEV_NAME, + }, + .st_mlc_probe = true, + .st_shub_probe = true, + .st_power_mode = true, + }, + { + .id = { + .hw_id = ST_ASM330LHH_ID, + .name = ST_ASM330LHH_DEV_NAME, + }, + }, +}; + +static const struct st_asm330lhhx_fs_table_entry st_asm330lhhx_fs_table[] = { + [ST_ASM330LHHX_ID_ACC] = { + .size = ST_ASM330LHHX_FS_ACC_LIST_SIZE, + .fs_avl[0] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ASM330LHHX_ACC_FS_2G_GAIN, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ASM330LHHX_ACC_FS_4G_GAIN, + .val = 0x2, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ASM330LHHX_ACC_FS_8G_GAIN, + .val = 0x3, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ASM330LHHX_ACC_FS_16G_GAIN, + .val = 0x1, + }, + }, + [ST_ASM330LHHX_ID_GYRO] = { + .size = ST_ASM330LHHX_FS_GYRO_LIST_SIZE, + .fs_avl[0] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ASM330LHHX_GYRO_FS_125_GAIN, + .val = 0x02, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ASM330LHHX_GYRO_FS_250_GAIN, + .val = 0x0, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ASM330LHHX_GYRO_FS_500_GAIN, + .val = 0x4, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ASM330LHHX_GYRO_FS_1000_GAIN, + .val = 0x8, + }, + .fs_avl[4] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ASM330LHHX_GYRO_FS_2000_GAIN, + .val = 0x0C, + }, + .fs_avl[5] = { + .reg = { + .addr = ST_ASM330LHHX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ASM330LHHX_GYRO_FS_4000_GAIN, + .val = 0x1, + }, + }, + [ST_ASM330LHHX_ID_TEMP] = { + .size = ST_ASM330LHHX_FS_TEMP_LIST_SIZE, + .fs_avl[0] = { + .gain = ST_ASM330LHHX_TEMP_FS_GAIN, + .val = 0x0 + }, + }, +}; + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES +static const struct st_asm330lhhx_ff_th st_asm330lhhx_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_asm330lhhx_6D_th st_asm330lhhx_6D_threshold[] = { + [0] = { + .val = 0x00, + .deg = 80, + }, + [1] = { + .val = 0x01, + .deg = 70, + }, + [2] = { + .val = 0x02, + .deg = 60, + }, + [3] = { + .val = 0x03, + .deg = 50, + }, +}; +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ + +static const inline struct iio_mount_matrix * +st_asm330lhhx_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *chan) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct iio_chan_spec_ext_info st_asm330lhhx_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, st_asm330lhhx_get_mount_matrix), + {}, +}; + +#define IIO_CHAN_HW_TIMESTAMP(si) { \ + .type = IIO_COUNT, \ + .address = ST_ASM330LHHX_REG_TIMESTAMP0_ADDR, \ + .scan_index = si, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 64, \ + .storagebits = 64, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_asm330lhhx_acc_channels[] = { + ST_ASM330LHHX_DATA_CHANNEL(IIO_ACCEL, ST_ASM330LHHX_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', st_asm330lhhx_ext_info), + ST_ASM330LHHX_DATA_CHANNEL(IIO_ACCEL, ST_ASM330LHHX_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', st_asm330lhhx_ext_info), + ST_ASM330LHHX_DATA_CHANNEL(IIO_ACCEL, ST_ASM330LHHX_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', st_asm330lhhx_ext_info), + ST_ASM330LHHX_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_asm330lhhx_gyro_channels[] = { + ST_ASM330LHHX_DATA_CHANNEL(IIO_ANGL_VEL, ST_ASM330LHHX_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', st_asm330lhhx_ext_info), + ST_ASM330LHHX_DATA_CHANNEL(IIO_ANGL_VEL, ST_ASM330LHHX_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', st_asm330lhhx_ext_info), + ST_ASM330LHHX_DATA_CHANNEL(IIO_ANGL_VEL, ST_ASM330LHHX_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', st_asm330lhhx_ext_info), + ST_ASM330LHHX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static +__maybe_unused const struct iio_chan_spec st_asm330lhhx_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_ASM330LHHX_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_ASM330LHHX_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_HW_TIMESTAMP(1), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +int __maybe_unused st_asm330lhhx_read_with_mask(struct st_asm330lhhx_hw *hw, u8 addr, u8 mask, + u8 *val) +{ + u8 data; + int err; + + err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data)); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + + goto out; + } + + *val = (data & mask) >> __ffs(mask); + +out: + return (err < 0) ? err : 0; +} + +int st_asm330lhhx_of_get_pin(struct st_asm330lhhx_hw *hw, int *pin) +{ + if (!dev_fwnode(hw->dev)) + return -EINVAL; + + return device_property_read_u32(hw->dev, "st,int-pin", pin); +} + +static int st_asm330lhhx_get_int_reg(struct st_asm330lhhx_hw *hw, u8 *drdy_reg) +{ + int err = 0, int_pin; + + if (st_asm330lhhx_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_ASM330LHHX_REG_INT1_CTRL_ADDR; + break; + case 2: + *drdy_reg = ST_ASM330LHHX_REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + hw->int_pin = int_pin; + + return err; +} + +static int __maybe_unused st_asm330lhhx_bk_regs(struct st_asm330lhhx_hw *hw) +{ + unsigned int data; + bool restore = 0; + int i, err = 0; + + mutex_lock(&hw->page_lock); + + for (i = 0; i < ST_ASM330LHHX_SUSPEND_RESUME_REGS; i++) { + if (st_asm330lhhx_suspend_resume[i].page != FUNC_CFG_ACCESS_0) { + /* skip if not support mlc */ + if (!hw->settings->st_mlc_probe) + continue; + + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR, + ST_ASM330LHHX_REG_ACCESS_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_ACCESS_MASK, + st_asm330lhhx_suspend_resume[i].page)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_asm330lhhx_suspend_resume[i].addr); + break; + } + + restore = 1; + } + + err = regmap_read(hw->regmap, + st_asm330lhhx_suspend_resume[i].addr, + &data); + if (err < 0) { + dev_err(hw->dev, + "failed to save register %02x\n", + st_asm330lhhx_suspend_resume[i].addr); + goto out_lock; + } + + if (restore) { + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR, + ST_ASM330LHHX_REG_ACCESS_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_ACCESS_MASK, + FUNC_CFG_ACCESS_0)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_asm330lhhx_suspend_resume[i].addr); + break; + } + + restore = 0; + } + + st_asm330lhhx_suspend_resume[i].val = data; + } + +out_lock: + mutex_unlock(&hw->page_lock); + + return err; +} + +static int __maybe_unused st_asm330lhhx_restore_regs(struct st_asm330lhhx_hw *hw) +{ + bool restore = 0; + int i, err = 0; + + mutex_lock(&hw->page_lock); + + for (i = 0; i < ST_ASM330LHHX_SUSPEND_RESUME_REGS; i++) { + if (st_asm330lhhx_suspend_resume[i].page != FUNC_CFG_ACCESS_0) { + /* skip if not support mlc */ + if (!hw->settings->st_mlc_probe) + continue; + + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR, + ST_ASM330LHHX_REG_ACCESS_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_ACCESS_MASK, + st_asm330lhhx_suspend_resume[i].page)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_asm330lhhx_suspend_resume[i].addr); + break; + } + + restore = 1; + } + + err = regmap_update_bits(hw->regmap, + st_asm330lhhx_suspend_resume[i].addr, + st_asm330lhhx_suspend_resume[i].mask, + st_asm330lhhx_suspend_resume[i].val); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_asm330lhhx_suspend_resume[i].addr); + break; + } + + if (restore) { + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR, + ST_ASM330LHHX_REG_ACCESS_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_ACCESS_MASK, + FUNC_CFG_ACCESS_0)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_asm330lhhx_suspend_resume[i].addr); + break; + } + + restore = 0; + } + } + + mutex_unlock(&hw->page_lock); + + return err; +} + +static int st_asm330lhhx_set_selftest( + struct st_asm330lhhx_sensor *sensor, int index) +{ + u8 mode, mask; + + switch (sensor->id) { + case ST_ASM330LHHX_ID_ACC: + mask = ST_ASM330LHHX_REG_ST_XL_MASK; + mode = st_asm330lhhx_selftest_table[index].accel_value; + break; + case ST_ASM330LHHX_ID_GYRO: + mask = ST_ASM330LHHX_REG_ST_G_MASK; + mode = st_asm330lhhx_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_asm330lhhx_update_bits_locked(sensor->hw, + ST_ASM330LHHX_REG_CTRL5_C_ADDR, + mask, mode); +} + +static ssize_t st_asm330lhhx_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_asm330lhhx_selftest_table[1].string_mode, + st_asm330lhhx_selftest_table[2].string_mode); +} + +static ssize_t st_asm330lhhx_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_asm330lhhx_sensor_id id = sensor->id; + + if (id != ST_ASM330LHHX_ID_ACC && + id != ST_ASM330LHHX_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); +} + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES +/* + * st_asm330lhhx_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 thershold register val = (th_ug * 2 ^ 6) / (1000000 * FS_XL) + */ +int st_asm330lhhx_set_wake_up_thershold(struct st_asm330lhhx_hw *hw, int th_ug) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev; + u8 val, fs_xl, max_th; + int tmp, err; + + err = st_asm330lhhx_read_with_mask(hw, + st_asm330lhhx_fs_table[ST_ASM330LHHX_ID_ACC].fs_avl[0].reg.addr, + st_asm330lhhx_fs_table[ST_ASM330LHHX_ID_ACC].fs_avl[0].reg.mask, + &fs_xl); + if (err < 0) + return err; + + tmp = (th_ug * 64) / (fs_xl * 1000000); + val = (u8)tmp; + max_th = ST_ASM330LHHX_WAKE_UP_THS_MASK >> + __ffs(ST_ASM330LHHX_WAKE_UP_THS_MASK); + if (val > max_th) + val = max_th; + + err = st_asm330lhhx_write_with_mask_locked(hw, + ST_ASM330LHHX_REG_WAKE_UP_THS_ADDR, + ST_ASM330LHHX_WAKE_UP_THS_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[0] = th_ug; + + return 0; +} + +/* + * st_asm330lhhx_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 + */ +int st_asm330lhhx_set_wake_up_duration(struct st_asm330lhhx_hw *hw, int dur_ms) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev; + int i, tmp, sensor_odr, err; + u8 val, odr_xl, max_dur; + + err = st_asm330lhhx_read_with_mask(hw, + st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].reg.addr, + st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].reg.mask, + &odr_xl); + if (err < 0) + return err; + + if (odr_xl == 0) { + dev_info(hw->dev, "use default ODR\n"); + odr_xl = st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].odr_avl[ST_ASM330LHHX_DEFAULT_XL_ODR_INDEX].val; + } + + for (i = 0; i < st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].size; i++) { + if (odr_xl == + st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].odr_avl[i].val) + break; + } + + if (i == st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].size) + return -EINVAL; + + + sensor_odr = ST_ASM330LHHX_ODR_EXPAND( + st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].odr_avl[i].hz, + st_asm330lhhx_odr_table[ST_ASM330LHHX_ID_ACC].odr_avl[i].uhz); + + tmp = dur_ms / (1000000 / (sensor_odr / 1000)); + val = (u8)tmp; + max_dur = ST_ASM330LHHX_WAKE_UP_DUR_MASK >> + __ffs(ST_ASM330LHHX_WAKE_UP_DUR_MASK); + if (val > max_dur) + val = max_dur; + + err = st_asm330lhhx_write_with_mask_locked(hw, + ST_ASM330LHHX_REG_WAKE_UP_DUR_ADDR, + ST_ASM330LHHX_WAKE_UP_DUR_MASK, + val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[1] = dur_ms; + + return 0; +} + +/* + * st_asm330lhhx_set_freefall_threshold - set free fall threshold detection mg + * @hw - ST IMU MEMS hw instance + * @th_mg - free fall threshold in mg + */ +int st_asm330lhhx_set_freefall_threshold(struct st_asm330lhhx_hw *hw, int th_mg) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_free_fall_threshold); i++) { + if (th_mg >= st_asm330lhhx_free_fall_threshold[i].mg) + break; + } + + if (i == ARRAY_SIZE(st_asm330lhhx_free_fall_threshold)) + return -EINVAL; + + err = st_asm330lhhx_write_with_mask_locked(hw, + ST_ASM330LHHX_REG_FREE_FALL_ADDR, + ST_ASM330LHHX_FF_THS_MASK, + st_asm330lhhx_free_fall_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_FF]; + sensor = iio_priv(iio_dev); + sensor->conf[2] = th_mg; + + return 0; +} + +/* + * st_asm330lhhx_set_6D_threshold - set 6D threshold detection in degrees + * @hw - ST IMU MEMS hw instance + * @deg - 6D threshold in degrees + */ +int st_asm330lhhx_set_6D_threshold(struct st_asm330lhhx_hw *hw, int deg) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_6D_threshold); i++) { + if (deg >= st_asm330lhhx_6D_threshold[i].deg) + break; + } + + if (i == ARRAY_SIZE(st_asm330lhhx_6D_threshold)) + return -EINVAL; + + err = st_asm330lhhx_write_with_mask_locked(hw, + ST_ASM330LHHX_REG_THS_6D_ADDR, + ST_ASM330LHHX_SIXD_THS_MASK, + st_asm330lhhx_6D_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_6D]; + sensor = iio_priv(iio_dev); + sensor->conf[3] = deg; + + return 0; +} +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ + +static __maybe_unused int st_asm330lhhx_reg_access(struct iio_dev *iio_dev, + unsigned int reg, unsigned int writeval, + unsigned int *readval) +{ + struct st_asm330lhhx_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_asm330lhhx_set_page_0(struct st_asm330lhhx_hw *hw) +{ + return regmap_write(hw->regmap, + ST_ASM330LHHX_REG_FUNC_CFG_ACCESS_ADDR, 0); +} + +static int st_asm330lhhx_check_whoami(struct st_asm330lhhx_hw *hw, + int id) +{ + int err, i; + int data; + + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_sensor_settings); i++) { + if (st_asm330lhhx_sensor_settings[i].id.name && + st_asm330lhhx_sensor_settings[i].id.hw_id == id) + break; + } + + if (i == ARRAY_SIZE(st_asm330lhhx_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_ASM330LHHX_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_ASM330LHHX_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + hw->settings = &st_asm330lhhx_sensor_settings[i]; + + return 0; +} + +static int st_asm330lhhx_get_odr_calibration(struct st_asm330lhhx_hw *hw) +{ + s64 odr_calib; + int data; + int err; + + err = regmap_read(hw->regmap, ST_ASM330LHHX_INTERNAL_FREQ_FINE, + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %d register\n", + ST_ASM330LHHX_INTERNAL_FREQ_FINE); + return err; + } + + odr_calib = ((s8)data * 37500) / 1000; + hw->ts_delta_ns = ST_ASM330LHHX_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_asm330lhhx_set_full_scale(struct st_asm330lhhx_sensor *sensor, + u32 gain) +{ + enum st_asm330lhhx_sensor_id id = sensor->id; + struct st_asm330lhhx_hw *hw = sensor->hw; + int i, err; + u8 val; + + /* for other sensors gain is fixed */ + if (id > ST_ASM330LHHX_ID_ACC) + return 0; + + for (i = 0; i < st_asm330lhhx_fs_table[id].size; i++) + if (st_asm330lhhx_fs_table[id].fs_avl[i].gain >= gain) + break; + + if (i == st_asm330lhhx_fs_table[id].size) + return -EINVAL; + + val = st_asm330lhhx_fs_table[id].fs_avl[i].val; + err = regmap_update_bits(hw->regmap, + st_asm330lhhx_fs_table[id].fs_avl[i].reg.addr, + st_asm330lhhx_fs_table[id].fs_avl[i].reg.mask, + ST_ASM330LHHX_SHIFT_VAL(val, + st_asm330lhhx_fs_table[id].fs_avl[i].reg.mask)); + if (err < 0) + return err; + + sensor->gain = st_asm330lhhx_fs_table[id].fs_avl[i].gain; + + return 0; +} + +int st_asm330lhhx_get_odr_val(enum st_asm330lhhx_sensor_id id, int odr, + int uodr, int *podr, int *puodr, u8 *val) +{ + int required_odr = ST_ASM330LHHX_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + if (required_odr == 0) { + *val = 0; + if (podr && puodr) { + *podr = 0; + *puodr = 0; + } + + return 0; + } + + for (i = 0; i < st_asm330lhhx_odr_table[id].size; i++) { + sensor_odr = ST_ASM330LHHX_ODR_EXPAND( + st_asm330lhhx_odr_table[id].odr_avl[i].hz, + st_asm330lhhx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= required_odr) + break; + } + + if (i == st_asm330lhhx_odr_table[id].size) + return -EINVAL; + + *val = st_asm330lhhx_odr_table[id].odr_avl[i].val; + + if (podr && puodr) { + *podr = st_asm330lhhx_odr_table[id].odr_avl[i].hz; + *puodr = st_asm330lhhx_odr_table[id].odr_avl[i].uhz; + } + + return 0; +} + +int __maybe_unused +st_asm330lhhx_get_odr_from_reg(enum st_asm330lhhx_sensor_id id, + u8 reg_val, u16 *podr, u32 *puodr) +{ + int i; + + for (i = 0; i < st_asm330lhhx_odr_table[id].size; i++) { + if (reg_val == st_asm330lhhx_odr_table[id].odr_avl[i].val) + break; + } + + if (i == st_asm330lhhx_odr_table[id].size) + return -EINVAL; + + *podr = st_asm330lhhx_odr_table[id].odr_avl[i].hz; + *puodr = st_asm330lhhx_odr_table[id].odr_avl[i].uhz; + + return 0; +} + +int st_asm330lhhx_get_batch_val(struct st_asm330lhhx_sensor *sensor, + int odr, int uodr, u8 *val) +{ + int required_odr = ST_ASM330LHHX_ODR_EXPAND(odr, uodr); + enum st_asm330lhhx_sensor_id id = sensor->id; + int sensor_odr; + int i; + + for (i = 0; i < st_asm330lhhx_odr_table[id].size; i++) { + sensor_odr = ST_ASM330LHHX_ODR_EXPAND( + st_asm330lhhx_odr_table[id].odr_avl[i].hz, + st_asm330lhhx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= required_odr) + break; + } + + if (i == st_asm330lhhx_odr_table[id].size) + return -EINVAL; + + *val = st_asm330lhhx_odr_table[id].odr_avl[i].batch_val; + + return 0; +} + +static u16 st_asm330lhhx_check_odr_dependency(struct st_asm330lhhx_hw *hw, + int odr, int uodr, + enum st_asm330lhhx_sensor_id ref_id) +{ + struct st_asm330lhhx_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = ST_ASM330LHHX_ODR_EXPAND(odr, uodr) > 0; + u16 ret; + + if (enable) { + /* uodr not used */ + if (hw->enable_mask & BIT_ULL(ref_id)) + ret = max_t(u16, ref->odr, odr); + else + ret = odr; + } else { + ret = (hw->enable_mask & BIT_ULL(ref_id)) ? ref->odr : 0; + } + + return ret; +} + +static int st_asm330lhhx_update_odr_fsm(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_sensor_id id, + enum st_asm330lhhx_sensor_id id_req, + int val, int delay) +{ + bool fsm_running = st_asm330lhhx_fsm_running(hw); + bool mlc_running = st_asm330lhhx_mlc_running(hw); + int ret = 0; + int status; + + if (fsm_running || mlc_running || + (id_req > ST_ASM330LHHX_ID_MLC)) { + /* + * In STMC_PAGE: + * Addr 0x02 bit 1 set to 1 -- CLK Disable + * Addr 0x05 bit 0 set to 0 -- FSM_EN=0 + * Addr 0x05 bit 4 set to 0 -- MLC_EN=0 + * Addr 0x67 bit 0 set to 0 -- FSM_INIT=0 + * Addr 0x67 bit 4 set to 0 -- MLC_INIT=0 + * Addr 0x02 bit 1 set to 0 -- CLK Disable + * - ODR change + * - Wait (~3 ODRs) + * In STMC_PAGE: + * Addr 0x05 bit 0 set to 1 -- FSM_EN = 1 + * Addr 0x05 bit 4 set to 1 -- MLC_EN = 1 + */ + mutex_lock(&hw->page_lock); + ret = st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + if (ret < 0) + goto unlock_page; + + ret = regmap_read(hw->regmap, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + &status); + if (ret < 0) + goto unlock_page; + + ret = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_PAGE_SEL_ADDR, + BIT(1), FIELD_PREP(BIT(1), 1)); + if (ret < 0) + goto unlock_page; + + ret = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_FSM_EN_MASK, + FIELD_PREP(ST_ASM330LHHX_FSM_EN_MASK, 0)); + if (ret < 0) + goto unlock_page; + + if (st_asm330lhhx_mlc_running(hw)) { + ret = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_MLC_EN_MASK, + FIELD_PREP(ST_ASM330LHHX_MLC_EN_MASK, 0)); + if (ret < 0) + goto unlock_page; + } + + ret = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_EMB_FUNC_INIT_B_ADDR, + ST_ASM330LHHX_MLC_INIT_MASK, + FIELD_PREP(ST_ASM330LHHX_MLC_INIT_MASK, 0)); + if (ret < 0) + goto unlock_page; + + ret = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_EMB_FUNC_INIT_B_ADDR, + ST_ASM330LHHX_FSM_INIT_MASK, + FIELD_PREP(ST_ASM330LHHX_FSM_INIT_MASK, 0)); + if (ret < 0) + goto unlock_page; + + ret = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_PAGE_SEL_ADDR, + BIT(1), FIELD_PREP(BIT(1), 0)); + if (ret < 0) + goto unlock_page; + + ret = st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + if (ret < 0) + goto unlock_page; + + ret = regmap_update_bits(hw->regmap, + st_asm330lhhx_odr_table[id].reg.addr, + st_asm330lhhx_odr_table[id].reg.mask, + ST_ASM330LHHX_SHIFT_VAL(val, + st_asm330lhhx_odr_table[id].reg.mask)); + if (ret < 0) + goto unlock_page; + + usleep_range(delay, delay + (delay / 10)); + + st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + + ret = regmap_write(hw->regmap, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + status); +unlock_page: + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + } else { + ret = st_asm330lhhx_update_bits_locked(hw, + st_asm330lhhx_odr_table[id].reg.addr, + st_asm330lhhx_odr_table[id].reg.mask, + val); + } + + return ret; +} + +static int st_asm330lhhx_set_odr(struct st_asm330lhhx_sensor *sensor, + int req_odr, int req_uodr) +{ + enum st_asm330lhhx_sensor_id id_req = sensor->id; + enum st_asm330lhhx_sensor_id id = sensor->id; + struct st_asm330lhhx_hw *hw = sensor->hw; + int err, delay; + u8 val = 0; + + switch (id) { + case ST_ASM330LHHX_ID_EXT0: + case ST_ASM330LHHX_ID_EXT1: + case ST_ASM330LHHX_ID_MLC_0: + case ST_ASM330LHHX_ID_MLC_1: + case ST_ASM330LHHX_ID_MLC_2: + case ST_ASM330LHHX_ID_MLC_3: + case ST_ASM330LHHX_ID_MLC_4: + case ST_ASM330LHHX_ID_MLC_5: + case ST_ASM330LHHX_ID_MLC_6: + case ST_ASM330LHHX_ID_MLC_7: + case ST_ASM330LHHX_ID_FSM_0: + case ST_ASM330LHHX_ID_FSM_1: + case ST_ASM330LHHX_ID_FSM_2: + case ST_ASM330LHHX_ID_FSM_3: + case ST_ASM330LHHX_ID_FSM_4: + case ST_ASM330LHHX_ID_FSM_5: + case ST_ASM330LHHX_ID_FSM_6: + case ST_ASM330LHHX_ID_FSM_7: + case ST_ASM330LHHX_ID_FSM_8: + case ST_ASM330LHHX_ID_FSM_9: + case ST_ASM330LHHX_ID_FSM_10: + case ST_ASM330LHHX_ID_FSM_11: + case ST_ASM330LHHX_ID_FSM_12: + case ST_ASM330LHHX_ID_FSM_13: + case ST_ASM330LHHX_ID_FSM_14: + case ST_ASM330LHHX_ID_FSM_15: + case ST_ASM330LHHX_ID_WK: + case ST_ASM330LHHX_ID_FF: + case ST_ASM330LHHX_ID_SC: + case ST_ASM330LHHX_ID_6D: + case ST_ASM330LHHX_ID_TEMP: + case ST_ASM330LHHX_ID_ACC: { + int odr; + int i; + + id = ST_ASM330LHHX_ID_ACC; + for (i = ST_ASM330LHHX_ID_ACC; i < ST_ASM330LHHX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + if (i == sensor->id) + continue; + + odr = st_asm330lhhx_check_odr_dependency(hw, req_odr, + req_uodr, i); + if (odr != req_odr) { + /* device already configured */ + return 0; + } + } + break; + } + case ST_ASM330LHHX_ID_GYRO: + break; + default: + return 0; + } + + err = st_asm330lhhx_get_odr_val(id, req_odr, req_uodr, NULL, + NULL, &val); + if (err < 0) + return err; + + /* check if sensor supports power mode setting */ + if (sensor->pm != ST_ASM330LHHX_NO_MODE && + hw->settings->st_power_mode) { + err = regmap_update_bits(hw->regmap, + st_asm330lhhx_odr_table[id].pm.addr, + st_asm330lhhx_odr_table[id].pm.mask, + ST_ASM330LHHX_SHIFT_VAL(sensor->pm, + st_asm330lhhx_odr_table[id].pm.mask)); + if (err < 0) + return err; + } + + delay = req_odr > 0 ? 4000000 / req_odr : 0; + + return st_asm330lhhx_update_odr_fsm(hw, id, id_req, val, delay); +} + +int st_asm330lhhx_sensor_set_enable(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_asm330lhhx_set_odr(sensor, odr, uodr); + if (err < 0) + return err; + + if (enable) + sensor->hw->enable_mask |= BIT_ULL(sensor->id); + else + sensor->hw->enable_mask &= ~BIT_ULL(sensor->id); + + return 0; +} + +static int st_asm330lhhx_read_oneshot(struct st_asm330lhhx_sensor *sensor, + u8 addr, int *val) +{ + struct st_asm330lhhx_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + err = st_asm330lhhx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + /* Use big delay for data valid because of drdy mask enabled */ + delay = 10000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_asm330lhhx_read_locked(hw, addr, + &data, + sizeof(data)); + if (err < 0) + return err; + + err = st_asm330lhhx_sensor_set_enable(sensor, false); + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_asm330lhhx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_asm330lhhx_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) + break; + + ret = st_asm330lhhx_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + 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_ASM330LHHX_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; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_asm330lhhx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_asm330lhhx_sensor *s = iio_priv(iio_dev); + int err; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_asm330lhhx_set_full_scale(s, val2); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + int todr, tuodr; + u8 data; + + err = st_asm330lhhx_get_odr_val(s->id, val, val2, + &todr, &tuodr, &data); + if (!err) { + s->odr = val; + s->uodr = tuodr; + + /* + * VTS test testSamplingRateHotSwitchOperation not + * toggle the enable status of sensor after changing + * the ODR -> force it + */ + if (s->hw->enable_mask & BIT_ULL(s->id)) { + switch (s->id) { + case ST_ASM330LHHX_ID_GYRO: + case ST_ASM330LHHX_ID_ACC: + err = st_asm330lhhx_set_odr(s, s->odr, s->uodr); + if (err < 0) + break; + + st_asm330lhhx_update_batching(iio_dev, 1); + break; + default: + break; + } + } + } + break; + } + default: + err = -EINVAL; + break; + } + + return err; +} + +static ssize_t +st_asm330lhhx_sysfs_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_asm330lhhx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_asm330lhhx_odr_table[id].size; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_asm330lhhx_odr_table[id].odr_avl[i].hz, + st_asm330lhhx_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_asm330lhhx_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_asm330lhhx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_asm330lhhx_fs_table[id].size; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_asm330lhhx_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_asm330lhhx_sysfs_get_power_mode_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + int i, len = 0; + + /* check for supported feature */ + if (hw->settings->st_power_mode) { + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_power_mode); i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%s ", + st_asm330lhhx_power_mode[i].string_mode); + } + } else { + len += scnprintf(buf + len, PAGE_SIZE - len, "%s ", + st_asm330lhhx_power_mode[0].string_mode); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_asm330lhhx_get_power_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%s\n", + st_asm330lhhx_power_mode[sensor->pm].string_mode); +} + +static ssize_t +st_asm330lhhx_set_power_mode(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + int err, i; + + /* check for supported feature */ + if (!hw->settings->st_power_mode) + return -EINVAL; + + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_power_mode); i++) { + if (strncmp(buf, st_asm330lhhx_power_mode[i].string_mode, + strlen(st_asm330lhhx_power_mode[i].string_mode)) == 0) + break; + } + + if (i == ARRAY_SIZE(st_asm330lhhx_power_mode)) + return -EINVAL; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + /* update power mode */ + sensor->pm = st_asm330lhhx_power_mode[i].val; + + iio_device_release_direct_mode(iio_dev); + + return size; +} + +static int st_asm330lhhx_selftest_sensor(struct st_asm330lhhx_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_ASM330LHHX_ID_ACC: + reg = ST_ASM330LHHX_REG_OUTX_L_A_ADDR; + bitmask = ST_ASM330LHHX_REG_STATUS_XLDA; + break; + case ST_ASM330LHHX_ID_GYRO: + reg = ST_ASM330LHHX_REG_OUTX_L_G_ADDR; + bitmask = ST_ASM330LHHX_REG_STATUS_GDA; + break; + default: + return -EINVAL; + } + + /* set selftest normal mode */ + ret = st_asm330lhhx_set_selftest(sensor, 0); + if (ret < 0) + return ret; + + ret = st_asm330lhhx_sensor_set_enable(sensor, true); + if (ret < 0) + return ret; + + /* wait at least 2 ODRs to be sure */ + delay = 2 * (1000000 / 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, 2 * delay); + ret = st_asm330lhhx_read_locked(sensor->hw, + ST_ASM330LHHX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_asm330lhhx_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; + } else { + try_count++; + } + } + } + + if (i != n) { + dev_err(sensor->hw->dev, + "some acc samples missing (expected %d, read %d)\n", + i, n); + ret = -1; + + goto selftest_failure; + } + + n = 0; + + /* set selftest mode */ + st_asm330lhhx_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, 2 * delay); + ret = st_asm330lhhx_read_locked(sensor->hw, + ST_ASM330LHHX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_asm330lhhx_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; + } else { + 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_asm330lhhx_set_selftest(sensor, 0); + + return st_asm330lhhx_sensor_set_enable(sensor, false); +} + +static ssize_t st_asm330lhhx_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_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + enum st_asm330lhhx_sensor_id id = sensor->id; + struct st_asm330lhhx_hw *hw = sensor->hw; + int ret, test; + u8 drdy_reg; + u32 gain; + + if (id != ST_ASM330LHHX_ID_ACC && + id != ST_ASM330LHHX_ID_GYRO) + return -EINVAL; + + for (test = 0; test < ARRAY_SIZE(st_asm330lhhx_selftest_table); test++) { + if (strncmp(buf, st_asm330lhhx_selftest_table[test].string_mode, + strlen(st_asm330lhhx_selftest_table[test].string_mode)) == 0) + break; + } + + if (test == ARRAY_SIZE(st_asm330lhhx_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_ULL(id)) { + ret = -EBUSY; + + goto out_claim; + } + + st_asm330lhhx_bk_regs(hw); + + /* disable FIFO watermak interrupt */ + ret = st_asm330lhhx_get_int_reg(hw, &drdy_reg); + if (ret < 0) + goto restore_regs; + + ret = st_asm330lhhx_update_bits_locked(hw, drdy_reg, + ST_ASM330LHHX_REG_INT_FIFO_TH_MASK, + 0); + if (ret < 0) + goto restore_regs; + + gain = sensor->gain; + if (id == ST_ASM330LHHX_ID_ACC) { + /* set BDU = 1, FS = 4 g, ODR = 52 Hz */ + st_asm330lhhx_set_full_scale(sensor, + ST_ASM330LHHX_ACC_FS_4G_GAIN); + st_asm330lhhx_set_odr(sensor, 52, 0); + st_asm330lhhx_selftest_sensor(sensor, test); + + /* restore full scale after test */ + st_asm330lhhx_set_full_scale(sensor, gain); + } else { + /* set BDU = 1, ODR = 208 Hz, FS = 2000 dps */ + st_asm330lhhx_set_full_scale(sensor, + ST_ASM330LHHX_GYRO_FS_2000_GAIN); + st_asm330lhhx_set_odr(sensor, 208, 0); + st_asm330lhhx_selftest_sensor(sensor, test); + + /* restore full scale after test */ + st_asm330lhhx_set_full_scale(sensor, gain); + } + +restore_regs: + st_asm330lhhx_restore_regs(hw); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +ssize_t st_asm330lhhx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_asm330lhhx_sysfs_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_asm330lhhx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_asm330lhhx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_asm330lhhx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_asm330lhhx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_asm330lhhx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_asm330lhhx_get_watermark, + st_asm330lhhx_set_watermark, 0); + +static IIO_DEVICE_ATTR(power_mode_available, 0444, + st_asm330lhhx_sysfs_get_power_mode_avail, NULL, 0); +static IIO_DEVICE_ATTR(power_mode, 0644, + st_asm330lhhx_get_power_mode, + st_asm330lhhx_set_power_mode, 0); + +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_asm330lhhx_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_asm330lhhx_sysfs_get_selftest_status, + st_asm330lhhx_sysfs_start_selftest, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_asm330lhhx_get_module_id, NULL, 0); + +static +ssize_t __maybe_unused st_asm330lhhx_get_discharded_samples(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + int ret; + + ret = sprintf(buf, "%d\n", sensor->discharged_samples); + + /* reset counter */ + sensor->discharged_samples = 0; + + return ret; +} + +static int st_asm330lhhx_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 IIO_DEVICE_ATTR(discharded_samples, 0444, + st_asm330lhhx_get_discharded_samples, NULL, 0); + +static struct attribute *st_asm330lhhx_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.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, + +#ifdef ST_ASM330LHHX_DEBUG_DISCHARGE + &iio_dev_attr_discharded_samples.dev_attr.attr, +#endif /* ST_ASM330LHHX_DEBUG_DISCHARGE */ + + NULL, +}; + +static const struct attribute_group st_asm330lhhx_acc_attribute_group = { + .attrs = st_asm330lhhx_acc_attributes, +}; + +static const struct iio_info st_asm330lhhx_acc_info = { + .attrs = &st_asm330lhhx_acc_attribute_group, + .read_raw = st_asm330lhhx_read_raw, + .write_raw = st_asm330lhhx_write_raw, + .write_raw_get_fmt = st_asm330lhhx_write_raw_get_fmt, +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_asm330lhhx_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static struct attribute *st_asm330lhhx_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.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, + +#ifdef ST_ASM330LHHX_DEBUG_DISCHARGE + &iio_dev_attr_discharded_samples.dev_attr.attr, +#endif /* ST_ASM330LHHX_DEBUG_DISCHARGE */ + + NULL, +}; + +static const struct attribute_group st_asm330lhhx_gyro_attribute_group = { + .attrs = st_asm330lhhx_gyro_attributes, +}; + +static const struct iio_info st_asm330lhhx_gyro_info = { + .attrs = &st_asm330lhhx_gyro_attribute_group, + .read_raw = st_asm330lhhx_read_raw, + .write_raw = st_asm330lhhx_write_raw, + .write_raw_get_fmt = st_asm330lhhx_write_raw_get_fmt, +}; + +static struct attribute *st_asm330lhhx_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_asm330lhhx_temp_attribute_group = { + .attrs = st_asm330lhhx_temp_attributes, +}; + +static const struct iio_info st_asm330lhhx_temp_info = { + .attrs = &st_asm330lhhx_temp_attribute_group, + .read_raw = st_asm330lhhx_read_raw, + .write_raw = st_asm330lhhx_write_raw, + .write_raw_get_fmt = st_asm330lhhx_write_raw_get_fmt, +}; + +static const unsigned long st_asm330lhhx_available_scan_masks[] = { + GENMASK(3, 0), 0x0 +}; + +static const unsigned long st_asm330lhhx_temp_available_scan_masks[] = { + GENMASK(1, 0), 0x0 +}; + +static int st_asm330lhhx_reset_device(struct st_asm330lhhx_hw *hw) +{ + int err; + + /* set configuration bit */ + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_CTRL9_XL_ADDR, + ST_ASM330LHHX_REG_DEVICE_CONF_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_DEVICE_CONF_MASK, 1)); + if (err < 0) + return err; + + /* sw reset */ + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_CTRL3_C_ADDR, + ST_ASM330LHHX_REG_SW_RESET_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_SW_RESET_MASK, 1)); + if (err < 0) + return err; + + msleep(50); + + /* boot */ + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_CTRL3_C_ADDR, + ST_ASM330LHHX_REG_BOOT_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_BOOT_MASK, 1)); + + msleep(50); + + return err; +} + +static int st_asm330lhhx_init_device(struct st_asm330lhhx_hw *hw) +{ + u8 drdy_reg; + int err; + + /* latch interrupts */ + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_TAP_CFG0_ADDR, + ST_ASM330LHHX_REG_LIR_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_LIR_MASK, 1)); + if (err < 0) + return err; + + /* enable Block Data Update */ + err = regmap_update_bits(hw->regmap, ST_ASM330LHHX_REG_CTRL3_C_ADDR, + ST_ASM330LHHX_REG_BDU_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_BDU_MASK, 1)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, ST_ASM330LHHX_REG_CTRL5_C_ADDR, + ST_ASM330LHHX_REG_ROUNDING_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_ROUNDING_MASK, 3)); + if (err < 0) + return err; + + /* init timestamp engine */ + err = regmap_update_bits(hw->regmap, + ST_ASM330LHHX_REG_CTRL10_C_ADDR, + ST_ASM330LHHX_REG_TIMESTAMP_EN_MASK, + ST_ASM330LHHX_SHIFT_VAL(true, + ST_ASM330LHHX_REG_TIMESTAMP_EN_MASK)); + if (err < 0) + return err; + + err = st_asm330lhhx_get_int_reg(hw, &drdy_reg); + if (err < 0) + return err; + + /* Enable DRDY MASK for filters settling time */ + err = regmap_update_bits(hw->regmap, ST_ASM330LHHX_REG_CTRL4_C_ADDR, + ST_ASM330LHHX_REG_DRDY_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_DRDY_MASK, + 1)); + + if (err < 0) + return err; + + /* enable FIFO watermak interrupt */ + return regmap_update_bits(hw->regmap, drdy_reg, + ST_ASM330LHHX_REG_INT_FIFO_TH_MASK, + FIELD_PREP(ST_ASM330LHHX_REG_INT_FIFO_TH_MASK, 1)); +} + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES +static int st_asm330lhhx_post_init_device(struct st_asm330lhhx_hw *hw) +{ + int err; + + /* Set default wake-up thershold to 93750 ug */ + err = st_asm330lhhx_set_wake_up_thershold(hw, 93750); + if (err < 0) + return err; + + /* Set default wake-up duration to 0 */ + err = st_asm330lhhx_set_wake_up_duration(hw, 0); + if (err < 0) + return err; + + /* setting default FF threshold to 312 mg */ + err = st_asm330lhhx_set_freefall_threshold(hw, 312); + if (err < 0) + return err; + + /* setting default 6D threshold to 60 degrees */ + return st_asm330lhhx_set_6D_threshold(hw, 60); +} +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ + +static struct iio_dev *st_asm330lhhx_alloc_iiodev(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_sensor_id id) +{ + struct st_asm330lhhx_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; + +#ifdef ST_ASM330LHHX_DEBUG_DISCHARGE + sensor->discharged_samples = 0; +#endif /* ST_ASM330LHHX_DEBUG_DISCHARGE */ + + /* + * for acc/gyro the default Android full scale settings are: + * Acc FS 8g (78.40 m/s^2) + * Gyro FS 1000dps (16.45 radians/sec) + */ + switch (id) { + case ST_ASM330LHHX_ID_ACC: + iio_dev->channels = st_asm330lhhx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_accel", hw->settings->id.name); + iio_dev->info = &st_asm330lhhx_acc_info; + iio_dev->available_scan_masks = + st_asm330lhhx_available_scan_masks; + sensor->max_watermark = ST_ASM330LHHX_MAX_FIFO_DEPTH; + sensor->gain = st_asm330lhhx_fs_table[id].fs_avl[ST_ASM330LHHX_DEFAULT_XL_FS_INDEX].gain; + sensor->odr = st_asm330lhhx_odr_table[id].odr_avl[ST_ASM330LHHX_DEFAULT_XL_ODR_INDEX].hz; + sensor->uodr = st_asm330lhhx_odr_table[id].odr_avl[ST_ASM330LHHX_DEFAULT_XL_ODR_INDEX].uhz; + sensor->offset = 0; + sensor->pm = ST_ASM330LHHX_HP_MODE; + sensor->min_st = ST_ASM330LHHX_SELFTEST_ACCEL_MIN; + sensor->max_st = ST_ASM330LHHX_SELFTEST_ACCEL_MAX; + break; + case ST_ASM330LHHX_ID_GYRO: + iio_dev->channels = st_asm330lhhx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_gyro_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_gyro", hw->settings->id.name); + iio_dev->info = &st_asm330lhhx_gyro_info; + iio_dev->available_scan_masks = + st_asm330lhhx_available_scan_masks; + sensor->max_watermark = ST_ASM330LHHX_MAX_FIFO_DEPTH; + sensor->gain = st_asm330lhhx_fs_table[id].fs_avl[ST_ASM330LHHX_DEFAULT_G_FS_INDEX].gain; + sensor->odr = st_asm330lhhx_odr_table[id].odr_avl[ST_ASM330LHHX_DEFAULT_G_ODR_INDEX].hz; + sensor->uodr = st_asm330lhhx_odr_table[id].odr_avl[ST_ASM330LHHX_DEFAULT_G_ODR_INDEX].uhz; + sensor->offset = 0; + sensor->pm = ST_ASM330LHHX_HP_MODE; + sensor->min_st = ST_ASM330LHHX_SELFTEST_GYRO_MIN; + sensor->max_st = ST_ASM330LHHX_SELFTEST_GYRO_MAX; + break; + case ST_ASM330LHHX_ID_TEMP: + iio_dev->channels = st_asm330lhhx_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", hw->settings->id.name); + iio_dev->info = &st_asm330lhhx_temp_info; + iio_dev->available_scan_masks = + st_asm330lhhx_temp_available_scan_masks; + sensor->max_watermark = ST_ASM330LHHX_MAX_FIFO_DEPTH; + sensor->gain = st_asm330lhhx_fs_table[id].fs_avl[ST_ASM330LHHX_DEFAULT_T_FS_INDEX].gain; + sensor->odr = st_asm330lhhx_odr_table[id].odr_avl[ST_ASM330LHHX_DEFAULT_T_ODR_INDEX].hz; + sensor->uodr = st_asm330lhhx_odr_table[id].odr_avl[ST_ASM330LHHX_DEFAULT_T_ODR_INDEX].uhz; + sensor->offset = ST_ASM330LHHX_TEMP_OFFSET; + sensor->pm = ST_ASM330LHHX_NO_MODE; + break; + default: + return NULL; + } + + st_asm330lhhx_set_full_scale(sensor, sensor->gain); + iio_dev->name = sensor->name; + + return iio_dev; +} + +static void st_asm330lhhx_get_properties(struct st_asm330lhhx_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +static void st_asm330lhhx_disable_regulator_action(void *_data) +{ + struct st_asm330lhhx_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static int st_asm330lhhx_power_enable(struct st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap) +{ + struct st_asm330lhhx_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->regmap = regmap; + hw->dev = dev; + hw->irq = irq; + hw->odr_table_entry = st_asm330lhhx_odr_table; + hw->hw_timestamp_global = 0; + + err = st_asm330lhhx_power_enable(hw); + if (err != 0) + return err; + + /* set page zero before access to registers */ + if (hw_id == ST_ASM330LHHX_ID) { + err = st_asm330lhhx_set_page_0(hw); + if (err < 0) + return err; + } + + err = st_asm330lhhx_check_whoami(hw, hw_id); + if (err < 0) + return err; + + st_asm330lhhx_get_properties(hw); + + err = st_asm330lhhx_get_odr_calibration(hw); + if (err < 0) + return err; + + err = st_asm330lhhx_reset_device(hw); + if (err < 0) + return err; + + err = st_asm330lhhx_init_device(hw); + if (err < 0) + return err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,15,0) + err = iio_read_mount_matrix(hw->dev, &hw->orientation); +#elif LINUX_VERSION_CODE >= KERNEL_VERSION(5,2,0) + err = iio_read_mount_matrix(hw->dev, "mount-matrix", &hw->orientation); +#else /* LINUX_VERSION_CODE */ + err = of_iio_read_mount_matrix(hw->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 = ST_ASM330LHHX_ID_GYRO; i <= ST_ASM330LHHX_ID_TEMP; i++) { + hw->iio_devs[i] = st_asm330lhhx_alloc_iiodev(hw, i); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + if (hw->settings->st_shub_probe) { + err = st_asm330lhhx_shub_probe(hw); + if (err < 0) + return err; + } + + if (hw->irq > 0) { + err = st_asm330lhhx_buffers_setup(hw); + if (err < 0) + return err; + } + + if (hw->settings->st_mlc_probe) { + err = st_asm330lhhx_mlc_probe(hw); + if (err < 0) + return err; + } + + for (i = ST_ASM330LHHX_ID_GYRO; i < ST_ASM330LHHX_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 (hw->settings->st_mlc_probe) { + err = st_asm330lhhx_mlc_init_preload(hw); + if (err) + return err; + } + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES + err = st_asm330lhhx_probe_event(hw); + if (err < 0) + return err; + + err = st_asm330lhhx_post_init_device(hw); + if (err < 0) + return err; +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ + + device_init_wakeup(dev, + device_property_read_bool(dev, "wakeup-source")); + + dev_info(dev, "Device probed\n"); + + return 0; +} +EXPORT_SYMBOL(st_asm330lhhx_probe); + +static int __maybe_unused st_asm330lhhx_suspend(struct device *dev) +{ + struct st_asm330lhhx_hw *hw = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Suspending device\n"); + + for (i = 0; i < ST_ASM330LHHX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT_ULL(sensor->id))) + continue; + + /* power off enabled sensors */ + err = st_asm330lhhx_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + + if (st_asm330lhhx_is_fifo_enabled(hw)) { + err = st_asm330lhhx_suspend_fifo(hw); + if (err < 0) + return err; + } + + err = st_asm330lhhx_bk_regs(hw); + + if (device_may_wakeup(dev)) + enable_irq_wake(hw->irq); + + return err < 0 ? err : 0; +} + +static int __maybe_unused st_asm330lhhx_resume(struct device *dev) +{ + struct st_asm330lhhx_hw *hw = dev_get_drvdata(dev); + struct st_asm330lhhx_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Resuming device\n"); + + if (device_may_wakeup(dev)) + disable_irq_wake(hw->irq); + + err = st_asm330lhhx_restore_regs(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_ASM330LHHX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT_ULL(sensor->id))) + continue; + + err = st_asm330lhhx_set_odr(sensor, sensor->odr, sensor->uodr); + if (err < 0) + return err; + } + + err = st_asm330lhhx_reset_hwts(hw); + if (err < 0) + return err; + + if (st_asm330lhhx_is_fifo_enabled(hw)) + err = st_asm330lhhx_set_fifo_mode(hw, ST_ASM330LHHX_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_asm330lhhx_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_asm330lhhx_suspend, st_asm330lhhx_resume) +}; +EXPORT_SYMBOL(st_asm330lhhx_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_asm330lhhx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_events.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_events.c new file mode 100644 index 000000000000..196d0ba10dfa --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_events.c @@ -0,0 +1,633 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx events function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_asm330lhhx.h" + +#ifdef CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES + +#define ST_ASM330LHHX_REG_ALL_INT_SRC_ADDR 0x1a +#define ST_ASM330LHHX_FF_IA_MASK BIT(0) +#define ST_ASM330LHHX_WU_IA_MASK BIT(1) +#define ST_ASM330LHHX_D6D_IA_MASK BIT(4) +#define ST_ASM330LHHX_SLEEP_CHANGE_MASK BIT(5) + +#define ST_ASM330LHHX_REG_WAKE_UP_SRC_ADDR 0x1b +#define ST_ASM330LHHX_WAKE_UP_EVENT_MASK GENMASK(3, 0) + +#define ST_ASM330LHHX_REG_D6D_SRC_ADDR 0x1d +#define ST_ASM330LHHX_D6D_EVENT_MASK GENMASK(5, 0) + +#define ST_ASM330LHHX_REG_INT_CFG1_ADDR 0x58 +#define ST_ASM330LHHX_INTERRUPTS_ENABLE_MASK BIT(7) + +#define ST_ASM330LHHX_REG_MD1_CFG_ADDR 0x5e +#define ST_ASM330LHHX_REG_MD2_CFG_ADDR 0x5f +#define ST_ASM330LHHX_INT_6D_MASK BIT(2) +#define ST_ASM330LHHX_INT_FF_MASK BIT(4) +#define ST_ASM330LHHX_INT_WU_MASK BIT(5) +#define ST_ASM330LHHX_INT_SLEEP_CHANGE_MASK BIT(7) + +static const unsigned long st_asm330lhhx_event_available_scan_masks[] = { + 0x1, 0x0 +}; + +static const struct iio_chan_spec st_asm330lhhx_wk_channels[] = { + { + .type = 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_asm330lhhx_ff_channels[] = { + ST_ASM330LHHX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_asm330lhhx_sc_channels[] = { + ST_ASM330LHHX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_asm330lhhx_6D_channels[] = { + { + .type = IIO_GESTURE, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 8, + .storagebits = 8, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static +int st_asm330lhhx_event_sensor_set_enable(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + int err, eint = !!enable; + struct st_asm330lhhx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_ASM330LHHX_REG_MD1_CFG_ADDR : + ST_ASM330LHHX_REG_MD2_CFG_ADDR; + + err = st_asm330lhhx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + switch (sensor->id) { + case ST_ASM330LHHX_ID_WK: + err = st_asm330lhhx_write_with_mask_locked(hw, int_reg, + ST_ASM330LHHX_INT_WU_MASK, + eint); + if (err < 0) + return err; + break; + case ST_ASM330LHHX_ID_FF: + err = st_asm330lhhx_write_with_mask_locked(hw, int_reg, + ST_ASM330LHHX_INT_FF_MASK, + eint); + if (err < 0) + return err; + break; + case ST_ASM330LHHX_ID_SC: + err = st_asm330lhhx_write_with_mask_locked(hw, int_reg, + ST_ASM330LHHX_INT_SLEEP_CHANGE_MASK, + eint); + if (err < 0) + return err; + break; + case ST_ASM330LHHX_ID_6D: + err = st_asm330lhhx_write_with_mask_locked(hw, int_reg, + ST_ASM330LHHX_INT_6D_MASK, + eint); + if (err < 0) + return err; + break; + default: + err = -EINVAL; + break; + } + + if (err >= 0) { + err = st_asm330lhhx_write_with_mask_locked(hw, + ST_ASM330LHHX_REG_INT_CFG1_ADDR, + ST_ASM330LHHX_INTERRUPTS_ENABLE_MASK, + eint); + if (eint == 0) + hw->enable_mask &= ~BIT_ULL(sensor->id); + else + hw->enable_mask |= BIT_ULL(sensor->id); + } + + return err; +} + +static int st_asm330lhhx_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_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT_ULL(sensor->id)); +} + +static int st_asm330lhhx_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_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_asm330lhhx_event_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +ssize_t st_asm330lhhx_wakeup_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[0]); +} + +ssize_t st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_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; +} + +ssize_t st_asm330lhhx_wakeup_duration_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[1]); +} + +ssize_t st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_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; +} + +ssize_t st_asm330lhhx_freefall_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[2]); +} + +ssize_t st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_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; +} + +ssize_t st_asm330lhhx_6D_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[3]); +} + +ssize_t st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_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_asm330lhhx_wakeup_threshold_get, + st_asm330lhhx_wakeup_threshold_set, 0); + +static IIO_DEVICE_ATTR(wakeup_duration, 0644, + st_asm330lhhx_wakeup_duration_get, + st_asm330lhhx_wakeup_duration_set, 0); + +static IIO_DEVICE_ATTR(freefall_threshold, 0644, + st_asm330lhhx_freefall_threshold_get, + st_asm330lhhx_freefall_threshold_set, 0); + +static IIO_DEVICE_ATTR(sixd_threshold, 0644, + st_asm330lhhx_6D_threshold_get, + st_asm330lhhx_6D_threshold_set, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_asm330lhhx_get_module_id, NULL, 0); + +static struct attribute *st_asm330lhhx_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_asm330lhhx_wk_attribute_group = { + .attrs = st_asm330lhhx_wk_attributes, +}; + +static const struct iio_info st_asm330lhhx_wk_info = { + .attrs = &st_asm330lhhx_wk_attribute_group, +}; + +static struct attribute *st_asm330lhhx_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_asm330lhhx_ff_attribute_group = { + .attrs = st_asm330lhhx_ff_attributes, +}; + +static const struct iio_info st_asm330lhhx_ff_info = { + .attrs = &st_asm330lhhx_ff_attribute_group, + .read_event_config = st_asm330lhhx_read_event_config, + .write_event_config = st_asm330lhhx_write_event_config, +}; + +static struct attribute *st_asm330lhhx_sc_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_asm330lhhx_sc_attribute_group = { + .attrs = st_asm330lhhx_sc_attributes, +}; + +static const struct iio_info st_asm330lhhx_sc_info = { + .attrs = &st_asm330lhhx_sc_attribute_group, + .read_event_config = st_asm330lhhx_read_event_config, + .write_event_config = st_asm330lhhx_write_event_config, +}; + +static struct attribute *st_asm330lhhx_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_asm330lhhx_6D_attribute_group = { + .attrs = st_asm330lhhx_6D_attributes, +}; + +static const struct iio_info st_asm330lhhx_6D_info = { + .attrs = &st_asm330lhhx_6D_attribute_group, +}; + +static +struct iio_dev *st_asm330lhhx_alloc_event_iiodev(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_sensor_id id) +{ + struct st_asm330lhhx_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->odr = 26; + iio_dev->available_scan_masks = st_asm330lhhx_event_available_scan_masks; + + switch (id) { + case ST_ASM330LHHX_ID_WK: + iio_dev->channels = st_asm330lhhx_wk_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_wk_channels); + iio_dev->name = "asm330lhhx_wk"; + iio_dev->info = &st_asm330lhhx_wk_info; + break; + case ST_ASM330LHHX_ID_FF: + iio_dev->channels = st_asm330lhhx_ff_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_ff_channels); + iio_dev->name = "asm330lhhx_ff"; + iio_dev->info = &st_asm330lhhx_ff_info; + break; + case ST_ASM330LHHX_ID_SC: + iio_dev->channels = st_asm330lhhx_sc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_sc_channels); + iio_dev->name = "asm330lhhx_sc"; + iio_dev->info = &st_asm330lhhx_sc_info; + break; + case ST_ASM330LHHX_ID_6D: + iio_dev->channels = st_asm330lhhx_6D_channels; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_6D_channels); + iio_dev->name = "asm330lhhx_6d"; + iio_dev->info = &st_asm330lhhx_6D_info; + break; + default: + iio_device_free(iio_dev); + + return NULL; + } + + return iio_dev; +} + +int st_asm330lhhx_event_handler(struct st_asm330lhhx_hw *hw) +{ + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + if (hw->enable_mask & + (BIT_ULL(ST_ASM330LHHX_ID_WK) | BIT_ULL(ST_ASM330LHHX_ID_FF) | + BIT_ULL(ST_ASM330LHHX_ID_SC) | BIT_ULL(ST_ASM330LHHX_ID_6D))) { + err = regmap_bulk_read(hw->regmap, + ST_ASM330LHHX_REG_ALL_INT_SRC_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* base function sensors */ + if (status & ST_ASM330LHHX_FF_IA_MASK) { + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_FF]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_asm330lhhx_get_time_ns(iio_dev)); + } + if (status & ST_ASM330LHHX_WU_IA_MASK) { + struct st_asm330lhhx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_WK]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + if (status & ST_ASM330LHHX_SLEEP_CHANGE_MASK) { + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_SC]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_asm330lhhx_get_time_ns(iio_dev)); + } + if (status & ST_ASM330LHHX_D6D_IA_MASK) { + struct st_asm330lhhx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_ASM330LHHX_ID_6D]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + } + + return IRQ_HANDLED; +} + +static inline int st_asm330lhhx_get_6D(struct st_asm330lhhx_hw *hw, u8 *out) +{ + return st_asm330lhhx_read_with_mask(hw, ST_ASM330LHHX_REG_D6D_SRC_ADDR, + ST_ASM330LHHX_D6D_EVENT_MASK, out); +} + +static inline int st_asm330lhhx_get_wk(struct st_asm330lhhx_hw *hw, u8 *out) +{ + return st_asm330lhhx_read_with_mask(hw, + ST_ASM330LHHX_REG_WAKE_UP_SRC_ADDR, + ST_ASM330LHHX_WAKE_UP_EVENT_MASK, out); +} + +static irqreturn_t st_asm330lhhx_6D_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + + st_asm330lhhx_get_6D(sensor->hw, &sensor->scan.event); + iio_push_to_buffers_with_timestamp(iio_dev, &sensor->scan.event, + st_asm330lhhx_get_time_ns(iio_dev)); + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +static irqreturn_t st_asm330lhhx_wk_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + + st_asm330lhhx_get_wk(sensor->hw, &sensor->scan.event); + iio_push_to_buffers_with_timestamp(iio_dev, &sensor->scan.event, + st_asm330lhhx_get_time_ns(iio_dev)); + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +int st_asm330lhhx_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig); + struct st_asm330lhhx_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_asm330lhhx_trigger_ops = { + .set_trigger_state = &st_asm330lhhx_trig_set_state, +}; + +static int st_asm330lhhx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_asm330lhhx_event_sensor_set_enable(iio_priv(iio_dev), true); +} + +static int st_asm330lhhx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_asm330lhhx_event_sensor_set_enable(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_asm330lhhx_buffer_ops = { + .preenable = st_asm330lhhx_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_asm330lhhx_buffer_postdisable, +}; + +int st_asm330lhhx_probe_event(struct st_asm330lhhx_hw *hw) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev; + irqreturn_t (*pthread[ST_ASM330LHHX_ID_MAX - ST_ASM330LHHX_ID_TRIGGER])(int irq, void *p) = { + [0] = st_asm330lhhx_wk_handler_thread, + [1] = st_asm330lhhx_6D_handler_thread, + /* add here all other trigger handler funcions */ + }; + int i, err; + + for (i = ST_ASM330LHHX_ID_EVENT; i < ST_ASM330LHHX_ID_MAX; i++) { + hw->iio_devs[i] = st_asm330lhhx_alloc_event_iiodev(hw, i); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + /* configure trigger sensors */ + for (i = ST_ASM330LHHX_ID_TRIGGER; i < ST_ASM330LHHX_ID_MAX; i++) { + iio_dev = hw->iio_devs[i]; + sensor = iio_priv(iio_dev); + + err = devm_iio_triggered_buffer_setup(hw->dev, iio_dev, + NULL, pthread[i - ST_ASM330LHHX_ID_TRIGGER], + &st_asm330lhhx_buffer_ops); + if (err < 0) + return err; + + sensor->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger", + iio_dev->name); + if (!sensor->trig) + return -ENOMEM; + + iio_trigger_set_drvdata(sensor->trig, iio_dev); + sensor->trig->ops = &st_asm330lhhx_trigger_ops; + sensor->trig->dev.parent = hw->dev; + iio_dev->trig = iio_trigger_get(sensor->trig); + + err = devm_iio_trigger_register(hw->dev, sensor->trig); + if (err) + return err; + } + + for (i = ST_ASM330LHHX_ID_EVENT; i < ST_ASM330LHHX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + return err; + } + + return 0; +} +#endif /* CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES */ diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_hwtimestamp.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_hwtimestamp.c new file mode 100644 index 000000000000..43e98b7dfc08 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_hwtimestamp.c @@ -0,0 +1,117 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx hwtimestamp library driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include "st_asm330lhhx.h" + +#define ST_ASM330LHHX_TSYNC_OFFSET_NS (300 * 1000LL) + +static void st_asm330lhhx_read_hw_timestamp(struct st_asm330lhhx_hw *hw) +{ + s64 timestamp_hw_global; + s64 eventLSB, eventMSB; + __le32 timestamp_hw; + s64 timestamp_cpu; + __le32 tmp; + int err; + + err = st_asm330lhhx_read_locked(hw, ST_ASM330LHHX_REG_TIMESTAMP0_ADDR, + (u8 *)×tamp_hw, + sizeof(timestamp_hw)); + if (err < 0) + return; + + timestamp_cpu = st_asm330lhhx_get_time_ns(hw->iio_devs[0]) - + ST_ASM330LHHX_TSYNC_OFFSET_NS; + + eventLSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 0, + IIO_EV_TYPE_TIME_SYNC, 0, 0, 0); + eventMSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 1, + 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_ASM330LHHX_ID_GYRO)) { + iio_push_event(hw->iio_devs[ST_ASM330LHHX_ID_GYRO], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_ASM330LHHX_ID_GYRO], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_ASM330LHHX_ID_ACC)) { + iio_push_event(hw->iio_devs[ST_ASM330LHHX_ID_ACC], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_ASM330LHHX_ID_ACC], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_ASM330LHHX_ID_TEMP)) { + iio_push_event(hw->iio_devs[ST_ASM330LHHX_ID_TEMP], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_ASM330LHHX_ID_TEMP], eventMSB, + timestamp_cpu); + } + + if (hw->timesync_c < 6) + hw->timesync_c++; + else + hw->timesync_ktime = ktime_set(0, ST_ASM330LHHX_DEFAULT_KTIME); +} + +static void st_asm330lhhx_timesync_fn(struct work_struct *work) +{ + struct st_asm330lhhx_hw *hw = container_of(work, struct st_asm330lhhx_hw, + timesync_work); + + st_asm330lhhx_read_hw_timestamp(hw); +} + +static enum hrtimer_restart st_asm330lhhx_timer_fn(struct hrtimer *timer) +{ + struct st_asm330lhhx_hw *hw; + + hw = container_of(timer, struct st_asm330lhhx_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_asm330lhhx_hwtimesync_init(struct st_asm330lhhx_hw *hw) +{ + hw->timesync_c = 0; + hw->timesync_ktime = ktime_set(0, ST_ASM330LHHX_DEFAULT_KTIME); + hrtimer_init(&hw->timesync_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + hw->timesync_timer.function = st_asm330lhhx_timer_fn; + + spin_lock_init(&hw->hwtimestamp_lock); + hw->hw_timestamp_global = 0; + + hw->timesync_workqueue = create_singlethread_workqueue("st_asm330_workqueue"); + if (!hw->timesync_workqueue) { + return -ENOMEM; + } + + INIT_WORK(&hw->timesync_work, st_asm330lhhx_timesync_fn); + + return 0; +} +EXPORT_SYMBOL(st_asm330lhhx_hwtimesync_init); diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_i2c.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_i2c.c new file mode 100644 index 000000000000..9808e3bcac51 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_i2c.c @@ -0,0 +1,75 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_asm330lhhx.h" + +static const struct regmap_config st_asm330lhhx_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_probe(&client->dev, client->irq, + hw_id, regmap); +} + +static const struct of_device_id st_asm330lhhx_i2c_of_match[] = { + { + .compatible = "st,asm330lhhx", + .data = (void *)ST_ASM330LHHX_ID, + }, + { + .compatible = "st,asm330lhh", + .data = (void *)ST_ASM330LHH_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_asm330lhhx_i2c_of_match); + +static const struct i2c_device_id st_asm330lhhx_i2c_id_table[] = { + { ST_ASM330LHHX_DEV_NAME, ST_ASM330LHHX_ID }, + { ST_ASM330LHH_DEV_NAME , ST_ASM330LHH_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_asm330lhhx_i2c_id_table); + +static struct i2c_driver st_asm330lhhx_driver = { + .driver = { + .name = "st_asm330lhhx_i2c", + .pm = &st_asm330lhhx_pm_ops, + .of_match_table = st_asm330lhhx_i2c_of_match, + }, + .probe = st_asm330lhhx_i2c_probe, + .id_table = st_asm330lhhx_i2c_id_table, +}; +module_i2c_driver(st_asm330lhhx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_asm330lhhx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_mlc.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_mlc.c new file mode 100644 index 000000000000..0a81d1e863c0 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_mlc.c @@ -0,0 +1,1001 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx machine learning core driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_asm330lhhx.h" + +#define ST_ASM330LHHX_MLC_LOADER_VERSION "0.8" + +/* number of machine learning core available on device hardware */ +#define ST_ASM330LHHX_MLC_NUMBER 8 +#define ST_ASM330LHHX_FSM_NUMBER 16 +#define ST_ASM330LHHX_MLC_FIRMWARE_NAME "st_asm330lhhx_mlc.bin" + +#ifdef CONFIG_IIO_ST_ASM330LHHX_MLC_PRELOAD +#include "st_asm330lhhx_preload_mlc.h" +#endif /* CONFIG_IIO_ST_ASM330LHHX_MLC_PRELOAD */ + +#define FSM_PAGE(__addr) ((u8)(((__addr >> 8) << 4) | 0x01)) +#define FSM_PAGE_MASK(__addr) ((u8)(__addr >> 8)) +#define FSM_OFFSET(__addr) ((u8)(__addr & 0x00FF)) + +/* specific events for MLC */ +#define ST_ASM330LHHX_MLC_FSM_TRSD_UPDATE 1 + +static struct iio_dev *st_asm330lhhx_mlc_alloc_iio_dev(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_sensor_id id); + +/* FSM / MLC IIO event channel definition */ +static const struct iio_chan_spec st_asm330lhhx_mlc_fsm_x_ch[] = { + ST_ASM330LHHX_EVENT_CHANNEL(IIO_ACTIVITY, thr), +}; + +static const unsigned long +st_asm330lhhx_fsm_mlc_available_scan_masks[] = { + BIT(0), 0x0 +}; + +/* remove old mlc/fsm configuration */ +static int st_asm330lhhx_mlc_purge_config(struct st_asm330lhhx_hw *hw) +{ + int err; + + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_MLC_EN_MASK, + ST_ASM330LHHX_SHIFT_VAL(0, ST_ASM330LHHX_MLC_EN_MASK)); + if (err < 0) + return err; + + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_FSM_EN_MASK, + ST_ASM330LHHX_SHIFT_VAL(0, ST_ASM330LHHX_FSM_EN_MASK)); + if (err < 0) + return err; + + /* wait ~10 ms */ + usleep_range(10000, 10100); + + return 0; +} + +static int +st_asm330lhhx_mlc_enable_sensor(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + struct st_asm330lhhx_hw *hw = sensor->hw; + int err = -ENODEV; + + if (sensor->id >= ST_ASM330LHHX_ID_MLC_0 && + sensor->id <= ST_ASM330LHHX_ID_MLC_7) { + int int_mlc_value; + int mlc_running; + + mlc_running = st_asm330lhhx_mlc_running(hw); + + int_mlc_value = enable ? hw->mlc_config->mlc_int_mask : 0; + if (hw->mlc_config->mlc_int_pin & BIT(0)) { + err = st_asm330lhhx_write_page_locked(hw, + ST_ASM330LHHX_MLC_INT1_ADDR, + &int_mlc_value, 1); + if (err < 0) + return err; + } + + if (hw->mlc_config->mlc_int_pin & BIT(1)) { + err = st_asm330lhhx_write_page_locked(hw, + ST_ASM330LHHX_MLC_INT2_ADDR, + &int_mlc_value, 1); + if (err < 0) + return err; + } + + err = st_asm330lhhx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + /* check for any other mlc already enabled */ + if ((!mlc_running && st_asm330lhhx_mlc_running(hw)) || + (mlc_running && !st_asm330lhhx_mlc_running(hw))) { + dev_info(sensor->hw->dev, "Reset MLC Algos\n"); + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_REG_EMB_FUNC_INIT_B_ADDR, + ST_ASM330LHHX_MLC_INIT_MASK, + ST_ASM330LHHX_SHIFT_VAL(1, + ST_ASM330LHHX_MLC_INIT_MASK)); + if (err < 0) + return err; + } + + dev_info(sensor->hw->dev, + "%s MLC sensor %d (INT %x)\n", + enable ? "Enabling" : "Disabling", + sensor->id, int_mlc_value); + } + + return err < 0 ? err : 0; +} + +static int +st_asm330lhhx_fsm_enable_sensor(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + struct st_asm330lhhx_hw *hw = sensor->hw; + int fsm_running = st_asm330lhhx_fsm_running(hw); + int id = sensor->id; + u8 mask, bitmask; + int err = 0; + + if (id >= ST_ASM330LHHX_ID_FSM_0 && + id < ST_ASM330LHHX_ID_FSM_8) { + mask = BIT(id - ST_ASM330LHHX_ID_FSM_0); + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_FSM_ENABLE_A_ADDR, + mask, + ST_ASM330LHHX_SHIFT_VAL(enable ? 1 : 0, + mask)); + if (err < 0) + return err; + + /* enable interrupts only if requested by ucf */ + bitmask = mask & hw->mlc_config->fsm_int_mask[0]; + if (bitmask) { + if (hw->mlc_config->mlc_int_pin & BIT(0)) { + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_FSM_INT1_A_ADDR, + bitmask, + ST_ASM330LHHX_SHIFT_VAL(enable ? 1 : 0, bitmask)); + if (err < 0) + return err; + } + + if (hw->mlc_config->mlc_int_pin & BIT(1)) { + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_FSM_INT2_A_ADDR, + bitmask, + ST_ASM330LHHX_SHIFT_VAL(enable ? 1 : 0, bitmask)); + if (err < 0) + return err; + } + } + + dev_info(sensor->hw->dev, + "%s FSM A sensor %d (INT %x)\n", + enable ? "Enabling" : "Disabling", + id, bitmask); + } else if (id >= ST_ASM330LHHX_ID_FSM_8 && + id < ST_ASM330LHHX_ID_FSM_15) { + mask = BIT(id - ST_ASM330LHHX_ID_FSM_8); + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_FSM_ENABLE_B_ADDR, + mask, + ST_ASM330LHHX_SHIFT_VAL(enable ? 1 : 0, + mask)); + if (err < 0) + return err; + + bitmask = mask & hw->mlc_config->fsm_int_mask[1]; + if (bitmask) { + if (hw->mlc_config->mlc_int_pin & BIT(0)) { + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_FSM_INT1_B_ADDR, + bitmask, + ST_ASM330LHHX_SHIFT_VAL(enable ? 1 : 0, bitmask)); + if (err < 0) + return err; + } + + if (hw->mlc_config->mlc_int_pin & BIT(1)) { + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_FSM_INT2_B_ADDR, + bitmask, + ST_ASM330LHHX_SHIFT_VAL(enable ? 1 : 0, bitmask)); + if (err < 0) + return err; + } + } + + dev_info(sensor->hw->dev, + "%s FSM B sensor %d (INT %x)\n", + enable ? "Enabling" : "Disabling", + id, bitmask); + } else { + dev_info(sensor->hw->dev, "Invalid fsm id %d\n", id); + + return -ENODEV; + } + + err = st_asm330lhhx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + /* check for any other fsm already enabled */ + if ((!fsm_running && st_asm330lhhx_fsm_running(hw)) || + (fsm_running && !st_asm330lhhx_fsm_running(hw))) { + dev_info(sensor->hw->dev, "Reset FSM Algos\n"); + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_REG_EMB_FUNC_INIT_B_ADDR, + ST_ASM330LHHX_FSM_INIT_MASK, + ST_ASM330LHHX_SHIFT_VAL(1, + ST_ASM330LHHX_FSM_INIT_MASK)); + if (err < 0) + return err; + + err = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_FSM_EN_MASK, + ST_ASM330LHHX_SHIFT_VAL(enable, + ST_ASM330LHHX_FSM_EN_MASK)); + if (err < 0) + return err; + } + + return 0; +} + +static int +st_asm330lhhx_mlc_fsm_enable_sensor(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + int err = 0; + + if (sensor->status == ST_ASM330LHHX_MLC_ENABLED) { + if (sensor->id == ST_ASM330LHHX_ID_MLC) { + sensor->hw->enable_mask &= ~BIT(sensor->id); + if (enable) + sensor->hw->enable_mask |= BIT(sensor->id); + } else { + err = st_asm330lhhx_mlc_enable_sensor(sensor, enable); + } + } else if (sensor->status == ST_ASM330LHHX_FSM_ENABLED) { + err = st_asm330lhhx_fsm_enable_sensor(sensor, enable); + } else { + return -ENODEV; + } + + return err < 0 ? err : 0; +} + +static int st_asm330lhhx_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_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + + return st_asm330lhhx_mlc_fsm_enable_sensor(sensor, state); +} + +static int st_asm330lhhx_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_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + struct st_asm330lhhx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/* parse and program mlc / fsm fragments */ +static int st_asm330lhhx_program_mlc(const struct firmware *fw, + struct st_asm330lhhx_hw *hw) +{ + u8 fsm_int[2] = { 0, 0 }, fsm_enable[2] = { 0, 0 }; + u8 mlc_fsm_en = 0, fsm_mlc_requested_odr = 0; + u8 mlc_int = 0, mlc_num = 0, fsm_num = 0; + bool stmc_page = false, skip = false; + int reg, val, ret, i = 0; + u32 uodr = 0; + u16 odr = 0; + + if (!hw->preload_mlc) { + ret = st_asm330lhhx_mlc_purge_config(hw); + if (ret < 0) + return ret; + } + + mutex_lock(&hw->page_lock); + + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == 0x01 && val == 0x80) { + stmc_page = true; + } else if (reg == 0x01 && val == 0x00) { + stmc_page = false; + } else if (stmc_page) { + /* catch configuration in stmc page */ + switch (reg) { + case ST_ASM330LHHX_MLC_INT1_ADDR: + case ST_ASM330LHHX_MLC_INT2_ADDR: + mlc_int |= val; + mlc_num = hweight8(mlc_int); + skip = true; + break; + case ST_ASM330LHHX_FSM_INT1_A_ADDR: + case ST_ASM330LHHX_FSM_INT2_A_ADDR: + fsm_int[0] |= val; + fsm_num = hweight16(*(u16 *)fsm_int); + skip = true; + break; + case ST_ASM330LHHX_FSM_INT1_B_ADDR: + case ST_ASM330LHHX_FSM_INT2_B_ADDR: + fsm_int[1] |= val; + fsm_num = hweight16(*(u16 *)fsm_int); + skip = true; + break; + case ST_ASM330LHHX_FSM_ENABLE_A_ADDR: + fsm_enable[0] |= val; + skip = true; + break; + case ST_ASM330LHHX_FSM_ENABLE_B_ADDR: + fsm_enable[1] |= val; + skip = true; + break; + case ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR: + /* + * check if mlc or fsm need to be + * enabled even if the interrupts are + * not used + */ + mlc_fsm_en |= val; + skip = true; + break; + default: + break; + } + } else if (!stmc_page) { + /* catch configuration in page 0 */ + switch (reg) { + /* skip FIFO and interrupt registers */ + case ST_ASM330LHHX_REG_FIFO_CTRL1_ADDR: + case ST_ASM330LHHX_REG_FIFO_CTRL2_ADDR: + case ST_ASM330LHHX_REG_FIFO_CTRL3_ADDR: + case ST_ASM330LHHX_REG_FIFO_CTRL4_ADDR: + case ST_ASM330LHHX_REG_INT1_CTRL_ADDR: + case ST_ASM330LHHX_REG_INT2_CTRL_ADDR: + case ST_ASM330LHHX_REG_CTRL3_C_ADDR: + case ST_ASM330LHHX_REG_CTRL4_C_ADDR: + case ST_ASM330LHHX_REG_CTRL5_C_ADDR: + case ST_ASM330LHHX_REG_CTRL6_C_ADDR: + case ST_ASM330LHHX_REG_CTRL7_G_ADDR: + case ST_ASM330LHHX_REG_CTRL10_C_ADDR: + skip = true; + break; + /* save requested odr for later */ + case ST_ASM330LHHX_CTRL1_XL_ADDR: + fsm_mlc_requested_odr = val >> 4; + skip = true; + break; + default: + break; + } + } + + if (!skip) { + ret = regmap_write(hw->regmap, reg, val); + if (ret) { + dev_err(hw->dev, "regmap_write fails\n"); + + goto unlock_page; + } + } + + skip = false; + + if (mlc_num >= ST_ASM330LHHX_MLC_NUMBER || + fsm_num >= ST_ASM330LHHX_FSM_NUMBER) + break; + } + + /* if MLC/FSM ODR is not configured uses first available */ + if (!fsm_mlc_requested_odr) + fsm_mlc_requested_odr = 0x01; + + ret = st_asm330lhhx_get_odr_from_reg(ST_ASM330LHHX_ID_ACC, + fsm_mlc_requested_odr, + &odr, &uodr); + if (ret < 0) { + fsm_num = 0; + mlc_num = 0; + + dev_err(hw->dev, + "unsupported ODR %d for MLC/FSM\n", + fsm_mlc_requested_odr); + + goto unlock_page; + } + + if (mlc_num) { + hw->mlc_config->mlc_int_mask = mlc_int; + hw->mlc_config->status |= ST_ASM330LHHX_MLC_ENABLED; + hw->mlc_config->mlc_configured = mlc_num; + } + + if (fsm_num) { + hw->mlc_config->fsm_int_mask[0] = fsm_int[0]; + hw->mlc_config->fsm_int_mask[1] = fsm_int[1]; + + hw->mlc_config->status |= ST_ASM330LHHX_FSM_ENABLED; + hw->mlc_config->fsm_configured = fsm_num; + + hw->mlc_config->fsm_enabled_mask[0] = fsm_enable[0]; + hw->mlc_config->fsm_enabled_mask[1] = fsm_enable[1]; + } + + hw->mlc_config->mlc_fsm_en = mlc_fsm_en; + hw->mlc_config->bin_len = fw->size; + hw->mlc_config->fsm_mlc_requested_odr = odr; + hw->mlc_config->fsm_mlc_requested_uodr = uodr; + +unlock_page: + mutex_unlock(&hw->page_lock); + + return (fsm_num + mlc_num) > 0 ? fsm_num + mlc_num : 0; +} + +static void st_asm330lhhx_mlc_update(const struct firmware *fw, + void *context) +{ + bool force_mlc_enabled, force_fsm_enabled = false; + struct st_asm330lhhx_hw *hw = context; + enum st_asm330lhhx_sensor_id id; + u16 fsm_mask = 0; + u8 mlc_mask = 0; + int ret, i; + + force_mlc_enabled = true; + + if (!fw) { + dev_err(hw->dev, "could not get binary firmware\n"); + return; + } + + mutex_lock(&hw->fifo_lock); + + ret = st_asm330lhhx_program_mlc(fw, hw); + if (!ret) + goto release; + + mlc_mask = hw->mlc_config->mlc_int_mask; + for (i = 0; i < ST_ASM330LHHX_MLC_NUMBER; i++) { + if (mlc_mask & BIT(i)) { + id = st_asm330lhhx_mlc_sensor_list[i]; + hw->iio_devs[id] = + st_asm330lhhx_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; + } + } + + fsm_mask = (u16)(((u16)hw->mlc_config->fsm_enabled_mask[1] << 8) | + hw->mlc_config->fsm_enabled_mask[0]); + for (i = 0; i < ST_ASM330LHHX_FSM_NUMBER; i++) { + if (fsm_mask & BIT(i)) { + id = st_asm330lhhx_fsm_sensor_list[i]; + hw->iio_devs[id] = + st_asm330lhhx_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; + } + } + + /* + * check if int are not configured but mlc/fsm need to + * be enabled + */ + if ((hw->mlc_config->mlc_fsm_en & ST_ASM330LHHX_MLC_EN_MASK) && + (!hw->mlc_config->mlc_int_mask)) + force_mlc_enabled = true; + + if ((hw->mlc_config->mlc_fsm_en & ST_ASM330LHHX_FSM_EN_MASK) && + (!hw->mlc_config->fsm_int_mask[0]) && + (!hw->mlc_config->fsm_int_mask[1])) + force_fsm_enabled = true; + + if (force_mlc_enabled) { + ret = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_MLC_EN_MASK, + ST_ASM330LHHX_SHIFT_VAL(true, + ST_ASM330LHHX_MLC_EN_MASK)); + if (ret < 0) + goto release; + } + + if (force_fsm_enabled) { + ret = st_asm330lhhx_update_page_bits_locked(hw, + ST_ASM330LHHX_EMB_FUNC_EN_B_ADDR, + ST_ASM330LHHX_FSM_EN_MASK, + ST_ASM330LHHX_SHIFT_VAL(true, + ST_ASM330LHHX_FSM_EN_MASK)); + if (ret < 0) + goto release; + } + + dev_info(hw->dev, "MLC loaded (%d) MLC %x FSM %x-%x (MLC %s FSM %s)\n", + ret, mlc_mask, + (fsm_mask >> 8) & 0xFF, fsm_mask & 0xFF, + force_mlc_enabled ? "Forced" : "On req", + force_fsm_enabled ? "Forced" : "On req"); + +release: + mutex_unlock(&hw->fifo_lock); + + if (hw->preload_mlc) { + hw->preload_mlc = 0; + + return; + } + + release_firmware(fw); +} + +static int st_asm330lhhx_mlc_flush_all(struct st_asm330lhhx_hw *hw) +{ + struct st_asm330lhhx_sensor *sensor_mlc; + struct iio_dev *iio_dev; + int ret = 0, id, i; + + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_mlc_sensor_list); i++) { + id = st_asm330lhhx_mlc_sensor_list[i]; + iio_dev = hw->iio_devs[id]; + if (!iio_dev) + continue; + + sensor_mlc = iio_priv(iio_dev); + ret = st_asm330lhhx_mlc_fsm_enable_sensor(sensor_mlc, + false); + if (ret < 0) + break; + + iio_device_unregister(iio_dev); + iio_device_free(iio_dev); + hw->iio_devs[id] = NULL; + } + + for (i = 0; i < ARRAY_SIZE(st_asm330lhhx_fsm_sensor_list); i++) { + id = st_asm330lhhx_fsm_sensor_list[i]; + iio_dev = hw->iio_devs[id]; + if (!iio_dev) + continue; + + sensor_mlc = iio_priv(iio_dev); + ret = st_asm330lhhx_mlc_fsm_enable_sensor(sensor_mlc, + false); + if (ret < 0) + break; + + iio_device_unregister(iio_dev); + iio_device_free(iio_dev); + hw->iio_devs[id] = NULL; + } + + return st_asm330lhhx_mlc_purge_config(hw); +} + +static ssize_t st_asm330lhhx_mlc_info(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_asm330lhhx_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_asm330lhhx_mlc_get_version(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "mlc loader Version %s\n", + ST_ASM330LHHX_MLC_LOADER_VERSION); +} + +static ssize_t +st_asm330lhhx_mlc_upload_firmware(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err; + + err = request_firmware_nowait(THIS_MODULE, true, + ST_ASM330LHHX_MLC_FIRMWARE_NAME, + dev, GFP_KERNEL, + sensor->hw, + st_asm330lhhx_mlc_update); + + return err < 0 ? err : size; +} + +static ssize_t st_asm330lhhx_mlc_flush(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_asm330lhhx_hw *hw = sensor->hw; + int ret; + + ret = st_asm330lhhx_mlc_flush_all(hw); + + hw->mlc_config->status = 0; + hw->mlc_config->fsm_configured = 0; + hw->mlc_config->mlc_configured = 0; + hw->mlc_config->mlc_fsm_en = 0; + hw->mlc_config->bin_len = 0; + hw->mlc_config->fsm_mlc_requested_odr = 0; + hw->mlc_config->fsm_mlc_requested_uodr = 0; + + return ret < 0 ? ret : size; +} + +static IIO_DEVICE_ATTR(mlc_info, 0444, + st_asm330lhhx_mlc_info, NULL, 0); +static IIO_DEVICE_ATTR(mlc_flush, 0200, + NULL, st_asm330lhhx_mlc_flush, 0); +static IIO_DEVICE_ATTR(mlc_version, 0444, + st_asm330lhhx_mlc_get_version, NULL, 0); +static IIO_DEVICE_ATTR(load_mlc, 0200, + NULL, st_asm330lhhx_mlc_upload_firmware, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_asm330lhhx_get_module_id, NULL, 0); + +static struct attribute *st_asm330lhhx_mlc_event_attributes[] = { + &iio_dev_attr_mlc_info.dev_attr.attr, + &iio_dev_attr_mlc_version.dev_attr.attr, + &iio_dev_attr_load_mlc.dev_attr.attr, + &iio_dev_attr_mlc_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_asm330lhhx_mlc_event_attribute_group = { + .attrs = st_asm330lhhx_mlc_event_attributes, +}; + +static const struct iio_info st_asm330lhhx_mlc_event_info = { + .attrs = &st_asm330lhhx_mlc_event_attribute_group, + .read_event_config = st_asm330lhhx_mlc_read_event_config, + .write_event_config = st_asm330lhhx_mlc_write_event_config, +}; + +static struct attribute *st_lsm6dsvx_mlc_x_event_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_asm330lhhx_mlc_x_attribute_group = { + .attrs = st_lsm6dsvx_mlc_x_event_attributes, +}; + +static const struct iio_info st_asm330lhhx_mlc_x_event_info = { + .attrs = &st_asm330lhhx_mlc_x_attribute_group, + .read_event_config = st_asm330lhhx_mlc_read_event_config, + .write_event_config = st_asm330lhhx_mlc_write_event_config, +}; + +static struct iio_dev * +st_asm330lhhx_mlc_alloc_iio_dev(struct st_asm330lhhx_hw *hw, + enum st_asm330lhhx_sensor_id id) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev = NULL; + + /* devm management only for ST_ASM330LHHX_ID_MLC */ + if (id == ST_ASM330LHHX_ID_MLC) { + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + } else { +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,9,0) + 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_ASM330LHHX_ID_MLC: + iio_dev->available_scan_masks = + st_asm330lhhx_fsm_mlc_available_scan_masks; + iio_dev->channels = st_asm330lhhx_mlc_fsm_x_ch; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_mlc_fsm_x_ch); + iio_dev->info = &st_asm330lhhx_mlc_event_info; + scnprintf(sensor->name, sizeof(sensor->name), "asm330lhhx_mlc"); + break; + case ST_ASM330LHHX_ID_MLC_0: + case ST_ASM330LHHX_ID_MLC_1: + case ST_ASM330LHHX_ID_MLC_2: + case ST_ASM330LHHX_ID_MLC_3: + case ST_ASM330LHHX_ID_MLC_4: + case ST_ASM330LHHX_ID_MLC_5: + case ST_ASM330LHHX_ID_MLC_6: + case ST_ASM330LHHX_ID_MLC_7: + iio_dev->available_scan_masks = + st_asm330lhhx_fsm_mlc_available_scan_masks; + iio_dev->channels = st_asm330lhhx_mlc_fsm_x_ch; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_mlc_fsm_x_ch); + iio_dev->info = &st_asm330lhhx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "asm330lhhx_mlc_%d", id - ST_ASM330LHHX_ID_MLC_0); + sensor->outreg_addr = ST_ASM330LHHX_REG_MLC0_SRC_ADDR + + id - ST_ASM330LHHX_ID_MLC_0; + sensor->status = ST_ASM330LHHX_MLC_ENABLED; + sensor->pm = ST_ASM330LHHX_NO_MODE; + sensor->odr = hw->mlc_config->fsm_mlc_requested_odr; + sensor->uodr = hw->mlc_config->fsm_mlc_requested_uodr; + break; + case ST_ASM330LHHX_ID_FSM_0: + case ST_ASM330LHHX_ID_FSM_1: + case ST_ASM330LHHX_ID_FSM_2: + case ST_ASM330LHHX_ID_FSM_3: + case ST_ASM330LHHX_ID_FSM_4: + case ST_ASM330LHHX_ID_FSM_5: + case ST_ASM330LHHX_ID_FSM_6: + case ST_ASM330LHHX_ID_FSM_7: + case ST_ASM330LHHX_ID_FSM_8: + case ST_ASM330LHHX_ID_FSM_9: + case ST_ASM330LHHX_ID_FSM_10: + case ST_ASM330LHHX_ID_FSM_11: + case ST_ASM330LHHX_ID_FSM_12: + case ST_ASM330LHHX_ID_FSM_13: + case ST_ASM330LHHX_ID_FSM_14: + case ST_ASM330LHHX_ID_FSM_15: + iio_dev->available_scan_masks = + st_asm330lhhx_fsm_mlc_available_scan_masks; + iio_dev->channels = st_asm330lhhx_mlc_fsm_x_ch; + iio_dev->num_channels = ARRAY_SIZE(st_asm330lhhx_mlc_fsm_x_ch); + iio_dev->info = &st_asm330lhhx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "asm330lhhx_fsm_%d", id - ST_ASM330LHHX_ID_FSM_0); + sensor->outreg_addr = ST_ASM330LHHX_FSM_OUTS1_ADDR + + id - ST_ASM330LHHX_ID_FSM_0; + sensor->status = ST_ASM330LHHX_FSM_ENABLED; + sensor->pm = ST_ASM330LHHX_NO_MODE; + sensor->odr = hw->mlc_config->fsm_mlc_requested_odr; + sensor->uodr = hw->mlc_config->fsm_mlc_requested_uodr; + break; + default: + dev_err(hw->dev, "invalid sensor id %d\n", id); + iio_device_free(iio_dev); + + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/* + * st_asm330lhhx_mlc_check_status - check for mlc/fsm events + * + * return: MLC/FSM index, < 0 for error + */ +int st_asm330lhhx_mlc_check_status(struct st_asm330lhhx_hw *hw) +{ + struct st_asm330lhhx_sensor *sensor; + struct iio_dev *iio_dev; + __le16 __fsm_status = 0; + int ret = 0, notify = 0; + u8 i, mlc_status, id; + u16 fsm_status; + + if (hw->mlc_config->status & ST_ASM330LHHX_MLC_ENABLED) { + ret = st_asm330lhhx_read_locked(hw, + ST_ASM330LHHX_MLC_STATUS_MAINPAGE, + (void *)&mlc_status, 1); + if (ret) + return ret; + + if (mlc_status) { + u8 mlc_event[ST_ASM330LHHX_MLC_NUMBER]; + + for (i = 0; i < ST_ASM330LHHX_MLC_NUMBER; i++) { + id = st_asm330lhhx_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) { + ret = -ENOENT; + + return ret; + } + + sensor = iio_priv(iio_dev); + ret = st_asm330lhhx_read_page_locked(hw, + sensor->outreg_addr, + (void *)&mlc_event[i], 1); + if (ret) + return ret; + + iio_push_event(iio_dev, (u64)mlc_event[i], + iio_get_time_ns(iio_dev)); + + dev_info(hw->dev, + "MLC %d Status %x MLC EVENT %llx\n", + id, mlc_status, + (u64)mlc_event[i]); + + notify |= BIT(i); + } + } + } + } + + if (hw->mlc_config->status & ST_ASM330LHHX_FSM_ENABLED) { + ret = st_asm330lhhx_read_locked(hw, + ST_ASM330LHHX_FSM_STATUS_A_MAINPAGE, + (void *)&__fsm_status, 2); + if (ret) + return ret; + + fsm_status = le16_to_cpu(__fsm_status); + if (fsm_status) { + u8 fsm_event[ST_ASM330LHHX_FSM_NUMBER]; + + for (i = 0; i < ST_ASM330LHHX_FSM_NUMBER; i++) { + id = st_asm330lhhx_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) { + ret = -ENOENT; + + return ret; + } + + sensor = iio_priv(iio_dev); + ret = st_asm330lhhx_read_page_locked(hw, + sensor->outreg_addr, + (void *)&fsm_event[i], 1); + if (ret) + return ret; + + iio_push_event(iio_dev, (u64)fsm_event[i], + iio_get_time_ns(iio_dev)); + + dev_info(hw->dev, + "FSM %d Status %x FSM EVENT %llx\n", + id, fsm_status, + (u64)fsm_event[i]); + + notify |= BIT(i + ST_ASM330LHHX_MLC_NUMBER); + } + } + } + } + + return ret < 0 ? ret : notify; +} + +/** + * st_asm330lhhx_of_get_mlc_int_pin - Read of configuration of mlc int + * + * @hw: Sensor hw structure. + * @pin: Interrupt pin used by MLC. + * + * Possible configurations are: + * st,mlc-int-pin = <1>; int1 pin will be used by MLC + * st,mlc-int-pin = <2>; int2 pin will be used by MLC + * st,mlc-int-pin = <3>; both interrupt pins will be used by MLC + */ +static int st_asm330lhhx_of_get_mlc_int_pin(struct st_asm330lhhx_hw *hw, + int *pin) +{ + struct device_node *np = hw->dev->of_node; + int int_pin; + int ret; + + if (!np) + return -EINVAL; + + ret = of_property_read_u32(np, "st,mlc-int-pin", &int_pin); + if (ret < 0) { + dev_info(hw->dev, + "missing mlc-int-pin, using default (%d)\n", + hw->int_pin); + int_pin = hw->int_pin; + } + + if (!(int_pin & 0x03)) { + dev_err(hw->dev, + "invalid mlc interrupt configuration (%d)\n", + int_pin); + + return -EINVAL; + } + + *pin = int_pin; + + return 0; +} + +int st_asm330lhhx_mlc_probe(struct st_asm330lhhx_hw *hw) +{ + int int_pin; + int ret; + + ret = st_asm330lhhx_of_get_mlc_int_pin(hw, &int_pin); + if (ret) + return -EINVAL; + + hw->iio_devs[ST_ASM330LHHX_ID_MLC] = + st_asm330lhhx_mlc_alloc_iio_dev(hw, + ST_ASM330LHHX_ID_MLC); + if (!hw->iio_devs[ST_ASM330LHHX_ID_MLC]) + return -ENOMEM; + + hw->mlc_config = devm_kzalloc(hw->dev, + sizeof(struct st_asm330lhhx_mlc_config_t), + GFP_KERNEL); + if (!hw->mlc_config) + return -ENOMEM; + + hw->mlc_config->mlc_int_pin = int_pin; + + return 0; +} + +int st_asm330lhhx_mlc_remove(struct device *dev) +{ + struct st_asm330lhhx_hw *hw = dev_get_drvdata(dev); + + return st_asm330lhhx_mlc_flush_all(hw); +} +EXPORT_SYMBOL(st_asm330lhhx_mlc_remove); + +int st_asm330lhhx_mlc_init_preload(struct st_asm330lhhx_hw *hw) +{ + +#ifdef CONFIG_IIO_ST_ASM330LHHX_MLC_PRELOAD + hw->preload_mlc = 1; + st_asm330lhhx_mlc_update(&st_asm330lhhx_mlc_preload, hw); +#endif /* CONFIG_IIO_ST_ASM330LHHX_MLC_PRELOAD */ + + return 0; +} diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_preload_mlc.h b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_preload_mlc.h new file mode 100644 index 000000000000..bc1e0cd6bfa5 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_preload_mlc.h @@ -0,0 +1,111 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_asm330lhhx mlc preload config + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_ASM330LHHX_PRELOAD_MLC_H +#define ST_ASM330LHHX_PRELOAD_MLC_H + +static const u8 mlcdata[] = { + /* File MLC_FSM_TOW_FSM.ucf */ + 0x10, 0x00, 0x11, 0x00, 0x01, 0x80, 0x04, 0x00, 0x05, 0x00, + 0x5f, 0x43, 0x46, 0x0f, 0x47, 0x00, 0x0a, 0x00, 0x0b, 0x07, + 0x0c, 0x00, 0x0e, 0x00, 0x0f, 0x08, 0x10, 0x00, 0x17, 0x40, + 0x09, 0x00, 0x02, 0x11, 0x08, 0x7a, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x04, 0x09, 0x04, 0x09, 0x00, 0x09, 0x04, 0x02, 0x41, + 0x08, 0x00, 0x09, 0x98, 0x09, 0x00, 0x09, 0x42, 0x09, 0x00, + 0x09, 0x12, 0x09, 0x00, 0x09, 0x66, 0x09, 0xae, 0x09, 0x66, + 0x09, 0x2e, 0x09, 0x80, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x11, 0x09, 0x00, 0x09, 0x58, 0x09, 0x00, 0x09, 0x23, + 0x09, 0x03, 0x09, 0x41, 0x09, 0x76, 0x09, 0x18, 0x09, 0x2c, + 0x09, 0x51, 0x09, 0x99, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x50, + 0x09, 0x81, 0x09, 0x99, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x80, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x98, 0x09, 0x00, 0x09, 0x42, + 0x09, 0x00, 0x09, 0x12, 0x09, 0x00, 0x09, 0x66, 0x09, 0xae, + 0x09, 0x60, 0x09, 0x2e, 0x09, 0x20, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x11, 0x09, 0x00, 0x09, 0x58, 0x09, 0x00, + 0x09, 0x23, 0x09, 0x03, 0x09, 0x41, 0x09, 0x76, 0x09, 0x18, + 0x09, 0x2c, 0x09, 0x51, 0x09, 0x99, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x50, 0x09, 0x81, 0x09, 0x99, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x80, 0x09, 0x00, 0x09, 0x00, 0x09, 0x98, 0x09, 0x00, + 0x09, 0x42, 0x09, 0x00, 0x09, 0x12, 0x09, 0x00, 0x09, 0x33, + 0x09, 0x3b, 0x09, 0x66, 0x09, 0x3c, 0x09, 0x08, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x11, 0x09, 0x00, 0x09, 0x58, + 0x09, 0x00, 0x09, 0x23, 0x09, 0x03, 0x09, 0x41, 0x09, 0x76, + 0x09, 0x18, 0x09, 0x2c, 0x09, 0x51, 0x09, 0x99, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x50, 0x09, 0x81, 0x09, 0x99, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x80, 0x09, 0x00, 0x09, 0x00, 0x09, 0x99, + 0x09, 0x10, 0x09, 0x44, 0x09, 0x00, 0x09, 0x14, 0x09, 0x00, + 0x09, 0xcd, 0x09, 0xb4, 0x09, 0xcd, 0x09, 0x34, 0x09, 0xa8, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x08, 0x09, 0x00, + 0x09, 0x58, 0x09, 0x00, 0x09, 0x08, 0x09, 0x00, 0x09, 0x23, + 0x09, 0x04, 0x09, 0x41, 0x09, 0x76, 0x09, 0x1a, 0x09, 0x2e, + 0x09, 0x16, 0x09, 0x3e, 0x09, 0x99, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x20, + 0x09, 0x17, 0x09, 0x3d, 0x09, 0x99, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x02, 0x51, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, + 0x09, 0x20, 0x09, 0x00, 0x09, 0x00, 0x04, 0x00, 0x05, 0x11, + 0x17, 0x00, 0x01, 0x00, 0x01, 0x00, 0x02, 0x3f, 0x04, 0x00, + 0x05, 0x00, 0x06, 0x00, 0x07, 0x00, 0x08, 0x00, 0x09, 0x00, + 0x0a, 0x00, 0x0b, 0x00, 0x0d, 0x00, 0x0e, 0x00, 0x10, 0x18, + 0x11, 0x00, 0x12, 0x44, 0x13, 0x00, 0x14, 0x00, 0x15, 0x00, + 0x16, 0x00, 0x17, 0x00, 0x18, 0xe0, 0x19, 0x00, 0x56, 0x00, + 0x57, 0x00, 0x58, 0x00, 0x59, 0x00, 0x5a, 0x00, 0x5b, 0x00, + 0x5c, 0x00, 0x5d, 0x00, 0x5e, 0x02, 0x5f, 0x02, 0x60, 0x00, + 0x61, 0x00, 0x62, 0x00, 0x73, 0x00, 0x74, 0x00, 0x75, 0x00, + 0x10, 0x00, 0x11, 0x00, 0x01, 0x80, 0x05, 0x00, 0x17, 0x40, + 0x02, 0x11, 0x08, 0xea, 0x09, 0x5c, 0x09, 0x03, 0x09, 0x74, + 0x09, 0x03, 0x09, 0x00, 0x09, 0x00, 0x09, 0x0a, 0x02, 0x11, + 0x08, 0xf2, 0x09, 0xff, 0x02, 0x11, 0x08, 0xfa, 0x09, 0x3c, + 0x09, 0x03, 0x09, 0x76, 0x09, 0x03, 0x09, 0x82, 0x09, 0x03, + 0x02, 0x31, 0x08, 0x3c, 0x09, 0x08, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0xad, 0x09, 0x35, 0x09, 0xad, 0x09, 0x35, 0x09, 0xa6, + 0x09, 0xb4, 0x09, 0x01, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x3f, 0x09, 0x00, + 0x09, 0x12, 0x09, 0x84, 0x09, 0x1f, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x76, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x01, 0x00, 0x12, 0x00, 0x01, 0x80, + 0x17, 0x40, 0x02, 0x09, 0x08, 0x00, 0x09, 0xcd, 0x09, 0x34, + 0x09, 0x01, 0x09, 0x00, 0x09, 0x00, 0x09, 0x0b, 0x09, 0x00, + 0x09, 0x00, 0x01, 0x80, 0x17, 0x00, 0x04, 0x00, 0x05, 0x10, + 0x03, 0x02, 0x02, 0x01, 0x01, 0x00, 0x12, 0x44, 0x01, 0x80, + 0x60, 0x05, 0x01, 0x00, 0x10, 0x18, 0x11, 0x00, 0x5e, 0x02, + 0x01, 0x80, 0x0d, 0x01, 0x01, 0x00, 0x01, 0x80, 0x17, 0x80, + 0x04, 0x00, 0x05, 0x11, 0x02, 0x01, 0x01, 0x00, +}; + +static struct firmware st_asm330lhhx_mlc_preload = { + .size = sizeof(mlcdata), + .data = mlcdata +}; + +#endif /* ST_ASM330LHHX_PRELOAD_MLC_H */ diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_shub.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_shub.c new file mode 100644 index 000000000000..bfedab009c41 --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_shub.c @@ -0,0 +1,1136 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx sensor hub library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_asm330lhhx.h" + +#define ST_ASM330LHHX_MAX_SLV_NUM 2 + +/** + * @struct st_asm330lhhx_ext_pwr + * @brief External device Power Management description + * reg: Generic sensor register description. + * off_val: Value to write into register to power off external sensor. + * on_val: Value to write into register for power on external sensor. + */ +struct st_asm330lhhx_ext_pwr { + struct st_asm330lhhx_reg reg; + u8 off_val; + u8 on_val; +}; + +/** + * @struct st_asm330lhhx_ext_dev_settings + * @brief External sensor descritor entry + * i2c_addr: External I2C device address (max two). + * wai_addr: Device ID address. + * wai_val: Device ID value. + * odr_table: ODR sensor table. + * fs_table: Full scale table. + * temp_comp_reg: Temperature compensation registers. + * pwr_table: External device Power Management description. + * off_canc_reg: Offset cancellation registers. + * bdu_reg: Block Data Update registers. + * ext_available_scan_masks: IIO device scan mask. + * ext_channels:IIO device channel specifications. + * ext_chan_depth: Max number of IIO device channel specifications. + * data_len: Sensor output data len. + */ +struct st_asm330lhhx_ext_dev_settings { + u8 i2c_addr[2]; + u8 wai_addr; + u8 wai_val; + struct st_asm330lhhx_odr_table_entry odr_table; + struct st_asm330lhhx_fs_table_entry fs_table; + struct st_asm330lhhx_reg temp_comp_reg; + struct st_asm330lhhx_ext_pwr pwr_table; + struct st_asm330lhhx_reg off_canc_reg; + struct st_asm330lhhx_reg bdu_reg; + unsigned long ext_available_scan_masks[2]; + const struct iio_chan_spec ext_channels[5]; + u8 ext_chan_depth; + u8 data_len; +}; + +static const struct +st_asm330lhhx_ext_dev_settings st_asm330lhhx_ext_dev_table[] = { + { + /* LIS2MDL */ + .i2c_addr = { 0x1e }, + .wai_addr = 0x4f, + .wai_val = 0x40, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + /* + * added 5Hz for CTS coverage, reg value + * is the samefor 5 and 10 Hz + */ + .odr_avl[0] = { 5, 1, 0x0, 0 }, + .odr_avl[1] = { 10, 0, 0x0, 0 }, + .odr_avl[2] = { 20, 0, 0x1, 0 }, + .odr_avl[3] = { 50, 0, 0x2, 0 }, + .odr_avl[4] = { 100, 0, 0x3, 0 }, + }, + .fs_table = { + .size = 1, + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + }, + .temp_comp_reg = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc_reg = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu_reg = { + .addr = 0x62, + .mask = BIT(4), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_ASM330LHHX_DATA_CHANNEL(IIO_MAGN, 0x68, + 1, IIO_MOD_X, 0, 16, 16, 's', NULL), + .ext_channels[1] = ST_ASM330LHHX_DATA_CHANNEL(IIO_MAGN, 0x6a, + 1, IIO_MOD_Y, 1, 16, 16, 's', NULL), + .ext_channels[2] = ST_ASM330LHHX_DATA_CHANNEL(IIO_MAGN, 0x6c, + 1, IIO_MOD_Z, 2, 16, 16, 's', NULL), + .ext_channels[3] = ST_ASM330LHHX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + { + /* LIS3MDL */ + .i2c_addr = { 0x1c, 0x1e }, + .wai_addr = 0x0f, + .wai_val = 0x3d, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x20, + .mask = GENMASK(4, 2), + }, + .odr_avl[0] = { 5, 0, 0x3, 0 }, + .odr_avl[1] = { 10, 0, 0x3, 0 }, + .odr_avl[2] = { 20, 0, 0x4, 0 }, + .odr_avl[3] = { 40, 0, 0x5, 0 }, + .odr_avl[4] = { 80, 0, 0x6, 0 }, + .odr_avl[5] = { 100, 0, 0x7, 0 }, + }, + .fs_table = { + .size = 4, + .fs_avl[0] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 6842, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 3421, + .val = 0x1, + }, + .fs_avl[2] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 2281, + .val = 0x2, + }, + .fs_avl[3] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 1711, + .val = 0x3, + }, + }, + .temp_comp_reg = { + .addr = 0x20, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x22, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .bdu_reg = { + .addr = 0x24, + .mask = BIT(6), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_ASM330LHHX_DATA_CHANNEL(IIO_MAGN, 0x28, + 1, IIO_MOD_X, 0, 16, 16, 's', NULL), + .ext_channels[1] = ST_ASM330LHHX_DATA_CHANNEL(IIO_MAGN, 0x2a, + 1, IIO_MOD_Y, 1, 16, 16, 's', NULL), + .ext_channels[2] = ST_ASM330LHHX_DATA_CHANNEL(IIO_MAGN, 0x2c, + 1, IIO_MOD_Z, 2, 16, 16, 's', NULL), + .ext_channels[3] = ST_ASM330LHHX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + { + /* LPS22HB */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb1, + .odr_table = { + .size = 4, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1, 0 }, + .odr_avl[1] = { 10, 0, 0x2, 0 }, + .odr_avl[2] = { 25, 0, 0x3, 0 }, + .odr_avl[3] = { 50, 0, 0x4, 0 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_ASM330LHHX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, 24, 32, 'u', NULL), + .ext_channels[1] = ST_ASM330LHHX_EVENT_CHANNEL(IIO_PRESSURE, flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, + { + /* LPS22HH */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb3, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1, 0 }, + .odr_avl[1] = { 10, 0, 0x2, 0 }, + .odr_avl[2] = { 25, 0, 0x3, 0 }, + .odr_avl[3] = { 50, 0, 0x4, 0 }, + .odr_avl[4] = { 100, 0, 0x6, 0 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_ASM330LHHX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, 24, 32, 'u', NULL), + .ext_channels[1] = ST_ASM330LHHX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, +}; + +/** + * Wait write trigger [SHUB] + * + * In write on external device register, each operation is triggered + * by accel/gyro data ready, this means that wait time depends on ODR + * plus i2c time + * NOTE: Be sure to enable Acc or Gyro before this operation + * + * @param hw: ST IMU MEMS hw instance. + */ +static inline void +st_asm330lhhx_shub_wait_complete(struct st_asm330lhhx_hw *hw) +{ + struct st_asm330lhhx_sensor *sensor; + int odr, uodr; + + sensor = iio_priv(hw->iio_devs[ST_ASM330LHHX_ID_ACC]); + + /* check if acc is enabled (it should be) */ + if (hw->enable_mask & BIT_ULL(ST_ASM330LHHX_ID_ACC)) { + odr = sensor->odr; + uodr = sensor->uodr; + } else { + odr = 12; + uodr = 500000; + } + + msleep((2000000000U / (odr * 1000000 + uodr)) + 1); +} + +/** + * Read from sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_asm330lhhx_shub_read_reg(struct st_asm330lhhx_hw *hw, + u8 addr, u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = regmap_bulk_read(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_SHUB_REG_MASK); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Write to sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_asm330lhhx_shub_write_reg(struct st_asm330lhhx_hw *hw, + u8 addr, u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = regmap_bulk_write(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_SHUB_REG_MASK); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable sensor hub interface [SHUB] + * + * NOTE: uses page_lock + * + * @param sensor: ST IMU sensor instance + * @param enable: Master Enable/Disable. + * @return 0 if OK, < 0 if ERROR + */ +static int st_asm330lhhx_shub_master_enable(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + struct st_asm330lhhx_hw *hw = sensor->hw; + int err; + + /* enable acc sensor as trigger */ + err = st_asm330lhhx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_asm330lhhx_set_page_access(hw, true, + ST_ASM330LHHX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = __st_asm330lhhx_write_with_mask(hw, + ST_ASM330LHHX_REG_MASTER_CONFIG_ADDR, + ST_ASM330LHHX_REG_MASTER_ON_MASK, + enable); + + st_asm330lhhx_set_page_access(hw, false, + ST_ASM330LHHX_REG_SHUB_REG_MASK); + +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Read sensor data register from shub interface + * + * NOTE: use SLV3 i2c slave for one-shot read operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_asm330lhhx_shub_read(struct st_asm330lhhx_sensor *sensor, + u8 addr, u8 *data, int len) +{ + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_asm330lhhx_hw *hw = sensor->hw; + u8 out_addr = ST_ASM330LHHX_REG_SLV0_OUT_ADDR + hw->ext_data_len; + u8 config[3]; + int err; + + config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[1] = addr; + config[2] = len & 0x7; + + err = st_asm330lhhx_shub_write_reg(hw, ST_ASM330LHHX_REG_SLV3_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_asm330lhhx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_asm330lhhx_shub_wait_complete(hw); + + err = st_asm330lhhx_shub_read_reg(hw, out_addr, data, len & 0x7); + + st_asm330lhhx_shub_master_enable(sensor, false); + + memset(config, 0, sizeof(config)); + + return st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_SLV3_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface + * + * NOTE: use SLV0 i2c slave for write operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_asm330lhhx_shub_write(struct st_asm330lhhx_sensor *sensor, + u8 addr, u8 *data, int len) +{ + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_asm330lhhx_hw *hw = sensor->hw; + u8 mconfig = ST_ASM330LHHX_REG_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + u8 config[3] = {}; + int err, i; + + /* AuxSens = 3 + wr once + pull up configuration */ + err = st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_MASTER_CONFIG_ADDR, + &mconfig, sizeof(mconfig)); + if (err < 0) + return err; + + config[0] = ext_info->ext_dev_i2c_addr << 1; + for (i = 0; i < len; i++) { + config[1] = addr + i; + + err = st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_DATAWRITE_SLV0_ADDR, + &data[i], 1); + if (err < 0) + return err; + + err = st_asm330lhhx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_asm330lhhx_shub_wait_complete(hw); + + st_asm330lhhx_shub_master_enable(sensor, false); + } + + return st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_SLV0_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface using register bitmask + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param mask: Register bitmask. + * @param val: Data buffer. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_asm330lhhx_shub_write_with_mask(struct st_asm330lhhx_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_asm330lhhx_shub_read(sensor, addr, + &data, sizeof(data)); + if (err < 0) + return err; + + data = (data & ~mask) | ST_ASM330LHHX_SHIFT_VAL(val, mask); + + return st_asm330lhhx_shub_write(sensor, addr, + &data, sizeof(data)); +} + +/** + * Configure external sensor connected on master I2C interface + * + * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_asm330lhhx_shub_config_channels(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + struct st_asm330lhhx_ext_dev_info *ext_info; + struct st_asm330lhhx_hw *hw = sensor->hw; + struct st_asm330lhhx_sensor *cur_sensor; + u8 config[6] = {}, enable_mask; + int i, j = 0; + + enable_mask = enable ? hw->enable_mask | BIT_ULL(sensor->id) + : hw->enable_mask & ~BIT_ULL(sensor->id); + + for (i = ST_ASM330LHHX_ID_EXT0; i <= ST_ASM330LHHX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT_ULL(cur_sensor->id))) + continue; + + ext_info = &cur_sensor->ext_dev_info; + config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[j + 1] = + ext_info->ext_dev_settings->ext_channels[0].address; + config[j + 2] = ST_ASM330LHHX_REG_BATCH_EXT_SENS_EN_MASK | + (ext_info->ext_dev_settings->data_len & + ST_ASM330LHHX_REG_SLAVE_NUMOP_MASK); + j += 3; + } + + return st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_SLV1_ADDR, + config, sizeof(config)); +} + +/** + * Get a valid ODR [SHUB] + * + * Check a valid ODR closest to the passed value + * + * @param sensor: SST IMU sensor instance. + * @param odr: ODR value (in Hz). + * @param val: ODR register value data pointer. + * @return 0 if OK, negative value for ERROR + */ +static int +st_asm330lhhx_shub_get_odr_val(struct st_asm330lhhx_sensor *sensor, + u16 odr, u8 *val) +{ + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) + if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr) + break; + + if (i == ext_info->ext_dev_settings->odr_table.size) + return -EINVAL; + + *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val; + + /* set decimator for low ODR */ + sensor->decimator = + ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz; + sensor->dec_counter = 0; + + return 0; +} + +/** + * Set new ODR to sensor [SHUB] + * + * Set a valid ODR closest to the passed value + * + * @param sensor: ST IMU sensor instance + * @param odr: ODR value (in Hz). + * @return 0 if OK, negative value for ERROR + */ +static int +st_asm330lhhx_shub_set_odr(struct st_asm330lhhx_sensor *sensor, u16 odr) +{ + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_asm330lhhx_hw *hw = sensor->hw; + u8 odr_val; + int err; + + err = st_asm330lhhx_shub_get_odr_val(sensor, odr, &odr_val); + if (err < 0) + return err; + + if (sensor->odr == odr && (hw->enable_mask & BIT_ULL(sensor->id))) + return 0; + + return st_asm330lhhx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + odr_val); +} + +/** + * Enable or Disable sensor [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable or disable the sensor [true,false]. + * @return 0 if OK, negative value for ERROR + */ +int +st_asm330lhhx_shub_set_enable(struct st_asm330lhhx_sensor *sensor, + bool enable) +{ + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err; + + err = st_asm330lhhx_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + if (enable) { + err = st_asm330lhhx_shub_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + } else { + err = st_asm330lhhx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + 0); + if (err < 0) + return err; + } + + if (ext_info->ext_dev_settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? + ext_info->ext_dev_settings->pwr_table.on_val : + ext_info->ext_dev_settings->pwr_table.off_val; + err = st_asm330lhhx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->pwr_table.reg.addr, + ext_info->ext_dev_settings->pwr_table.reg.mask, + val); + if (err < 0) + return err; + } + + return st_asm330lhhx_shub_master_enable(sensor, enable); +} + +static inline u32 st_asm330lhhx_get_unaligned_le24(const u8 *p) +{ + return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; +} + +/** + * Single sensor read operation [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param ch: IIO Channel. + * @param val: Output data register value. + * @return IIO_VAL_INT if OK, negative value for ERROR + */ +static int +st_asm330lhhx_shub_read_oneshot(struct st_asm330lhhx_sensor *sensor, + struct iio_chan_spec const *ch, + int *val) +{ + int err, delay, len = ch->scan_type.realbits >> 3; + u8 data[4]; + + if (len > ARRAY_SIZE(data)) + return -ENOMEM; + + err = st_asm330lhhx_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_asm330lhhx_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + st_asm330lhhx_shub_set_enable(sensor, false); + + switch (len) { + case 3: + *val = (s32)st_asm330lhhx_get_unaligned_le24(data); + break; + case 2: + *val = (s16)get_unaligned_le16(data); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +/** + * Read Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param ch: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_asm330lhhx_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_asm330lhhx_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_asm330lhhx_shub_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * Write Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int +st_asm330lhhx_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_asm330lhhx_shub_get_odr_val(sensor, val, + &data); + if (!err) + sensor->odr = val; + break; + } + case IIO_CHAN_INFO_SCALE: + err = 0; + break; + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Get a list of available sensor ODR [SHUB] + * + * List of available ODR returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_asm330lhhx_sysfs_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) { + u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, + "%d ", val); + } + buf[len - 1] = '\n'; + + return len; +} + +/** + * Get a list of available sensor Full Scale [SHUB] + * + * List of available Full Scale returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_asm330lhhx_sysfs_shub_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_asm330lhhx_sensor *sensor = + iio_priv(dev_get_drvdata(dev)); + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->fs_table.size; i++) { + u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, + "0.%06u ", val); + } + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_asm330lhhx_sysfs_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_ext_scale_available, 0444, + st_asm330lhhx_sysfs_shub_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_asm330lhhx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, + st_asm330lhhx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_asm330lhhx_get_watermark, + st_asm330lhhx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_asm330lhhx_get_module_id, NULL, 0); + +static struct attribute *st_asm330lhhx_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_ext_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_asm330lhhx_ext_attribute_group = { + .attrs = st_asm330lhhx_ext_attributes, +}; + +static const struct iio_info st_asm330lhhx_ext_info = { + .attrs = &st_asm330lhhx_ext_attribute_group, + .read_raw = st_asm330lhhx_shub_read_raw, + .write_raw = st_asm330lhhx_shub_write_raw, +}; + +/** + * Allocate IIO device [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @param ext_settings: xternal sensor descritor entry. + * @param id: Sensor Identifier. + * @param i2c_addr: external I2C address on master bus. + * @return struct iio_dev *, NULL if ERROR + */ +static struct iio_dev * +st_asm330lhhx_shub_alloc_iio_dev(struct st_asm330lhhx_hw *hw, + const struct st_asm330lhhx_ext_dev_settings *ext_settings, + enum st_asm330lhhx_sensor_id id, u8 i2c_addr) +{ + struct st_asm330lhhx_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; + iio_dev->available_scan_masks = + ext_settings->ext_available_scan_masks; + iio_dev->info = &st_asm330lhhx_ext_info; + iio_dev->channels = ext_settings->ext_channels; + iio_dev->num_channels = ext_settings->ext_chan_depth; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = ext_settings->odr_table.odr_avl[0].hz; + sensor->gain = ext_settings->fs_table.fs_avl[0].gain; + sensor->max_watermark = ST_ASM330LHHX_MAX_FIFO_DEPTH; + sensor->watermark = 1; + sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr; + sensor->ext_dev_info.ext_dev_settings = ext_settings; + sensor->decimator = 0; + sensor->dec_counter = 0; + sensor->pm = ST_ASM330LHHX_NO_MODE; + + switch (iio_dev->channels[0].type) { + case IIO_MAGN: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_magn", hw->settings->id.name); + break; + case IIO_PRESSURE: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", hw->settings->id.name); + break; + default: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ext", hw->settings->id.name); + break; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static int +st_asm330lhhx_shub_init_remote_sensor(struct st_asm330lhhx_sensor *sensor) +{ + struct st_asm330lhhx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err = 0; + + if (ext_info->ext_dev_settings->bdu_reg.addr) + err = st_asm330lhhx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->bdu_reg.addr, + ext_info->ext_dev_settings->bdu_reg.mask, + 1); + + if (ext_info->ext_dev_settings->temp_comp_reg.addr) + err = st_asm330lhhx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->temp_comp_reg.addr, + ext_info->ext_dev_settings->temp_comp_reg.mask, + 1); + + if (ext_info->ext_dev_settings->off_canc_reg.addr) + err = st_asm330lhhx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->off_canc_reg.addr, + ext_info->ext_dev_settings->off_canc_reg.mask, + 1); + + return err; +} + +/** + * Probe device function [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @return 0 if OK, negative for ERROR + */ +int st_asm330lhhx_shub_probe(struct st_asm330lhhx_hw *hw) +{ + const struct st_asm330lhhx_ext_dev_settings *settings; + struct st_asm330lhhx_sensor *acc_sensor, *sensor; + u8 config[3], data, num_ext_dev = 0; + enum st_asm330lhhx_sensor_id id; + int err, i = 0, j; + struct device_node *np = hw->dev->of_node; + + if (np && of_property_read_bool(np, "drive-pullup-shub")) { + dev_info(hw->dev, "enabling pull up on i2c master\n"); + err = st_asm330lhhx_shub_read_reg(hw, + ST_ASM330LHHX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + if (err < 0) + return err; + + data |= ST_ASM330LHHX_REG_SHUB_PU_EN_MASK; + err = st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + + if (err < 0) + return err; + + hw->i2c_master_pu = ST_ASM330LHHX_REG_SHUB_PU_EN_MASK; + } + + acc_sensor = iio_priv(hw->iio_devs[ST_ASM330LHHX_ID_ACC]); + while (i < ARRAY_SIZE(st_asm330lhhx_ext_dev_table) && + num_ext_dev < ST_ASM330LHHX_MAX_SLV_NUM) { + settings = &st_asm330lhhx_ext_dev_table[i]; + + for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) { + if (!settings->i2c_addr[j]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[j] << 1) | 1; + config[1] = settings->wai_addr; + config[2] = 1; + + err = st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_asm330lhhx_shub_master_enable(acc_sensor, + true); + if (err < 0) + return err; + + st_asm330lhhx_shub_wait_complete(hw); + + err = st_asm330lhhx_shub_read_reg(hw, + ST_ASM330LHHX_REG_SLV0_OUT_ADDR, + &data, sizeof(data)); + + st_asm330lhhx_shub_master_enable(acc_sensor, + false); + + if (err < 0) + return err; + + if (data != settings->wai_val) + continue; + + id = ST_ASM330LHHX_ID_EXT0 + num_ext_dev; + hw->iio_devs[id] = st_asm330lhhx_shub_alloc_iio_dev(hw, + settings, id, + settings->i2c_addr[j]); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_asm330lhhx_shub_init_remote_sensor(sensor); + if (err < 0) + return err; + + num_ext_dev++; + hw->ext_data_len += settings->data_len; + break; + } + + i++; + } + + if (!num_ext_dev) + return 0; + + memset(config, 0, sizeof(config)); + err = st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + /* AuxSens = 3 + wr once */ + data = ST_ASM330LHHX_REG_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + return st_asm330lhhx_shub_write_reg(hw, + ST_ASM330LHHX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); +} diff --git a/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_spi.c b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_spi.c new file mode 100644 index 000000000000..bbd19c2fdf2b --- /dev/null +++ b/drivers/iio/stm/imu/st_asm330lhhx/st_asm330lhhx_spi.c @@ -0,0 +1,74 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_asm330lhhx spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_asm330lhhx.h" + +static const struct regmap_config st_asm330lhhx_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_asm330lhhx_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_asm330lhhx_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_asm330lhhx_probe(&spi->dev, spi->irq, hw_id, regmap); +} + + +static const struct of_device_id st_asm330lhhx_spi_of_match[] = { + { + .compatible = "st,asm330lhhx", + .data = (void *)ST_ASM330LHHX_ID, + }, + { + .compatible = "st,asm330lhh", + .data = (void *)ST_ASM330LHH_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_asm330lhhx_spi_of_match); + +static const struct spi_device_id st_asm330lhhx_spi_id_table[] = { + { ST_ASM330LHHX_DEV_NAME, ST_ASM330LHHX_ID }, + { ST_ASM330LHH_DEV_NAME , ST_ASM330LHH_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_asm330lhhx_spi_id_table); + +static struct spi_driver st_asm330lhhx_driver = { + .driver = { + .name = "st_asm330lhhx_spi", + .pm = &st_asm330lhhx_pm_ops, + .of_match_table = st_asm330lhhx_spi_of_match, + }, + .probe = st_asm330lhhx_spi_probe, + .id_table = st_asm330lhhx_spi_id_table, +}; +module_spi_driver(st_asm330lhhx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_asm330lhhx spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_imu68/Kconfig b/drivers/iio/stm/imu/st_imu68/Kconfig new file mode 100644 index 000000000000..9ee5304805b3 --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/Kconfig @@ -0,0 +1,24 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_IMU68 + tristate "STMicroelectronics LSM9DS1 IMU sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_IMU68_I2C if (I2C) + select IIO_ST_IMU68_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics IMU sensors: + LSM9DS1 + + To compile this driver as a module, choose M here: the module + will be called st_imu68. + +config IIO_ST_IMU68_I2C + tristate + depends on IIO_ST_IMU68 + +config IIO_ST_IMU68_SPI + tristate + depends on IIO_ST_IMU68 + diff --git a/drivers/iio/stm/imu/st_imu68/Makefile b/drivers/iio/stm/imu/st_imu68/Makefile new file mode 100644 index 000000000000..88fdcdff9ea8 --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/Makefile @@ -0,0 +1,6 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_imu68-y := st_imu68_core.o st_imu68_buffer.o + +obj-$(CONFIG_IIO_ST_IMU68) += st_imu68.o +obj-$(CONFIG_IIO_ST_IMU68_I2C) += st_imu68_i2c.o +obj-$(CONFIG_IIO_ST_IMU68_SPI) += st_imu68_spi.o diff --git a/drivers/iio/stm/imu/st_imu68/st_imu68.h b/drivers/iio/stm/imu/st_imu68/st_imu68.h new file mode 100644 index 000000000000..4eb4f9c3ac2e --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/st_imu68.h @@ -0,0 +1,87 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_imu68 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_IMU68_H +#define ST_IMU68_H + +#include + +#define ST_LSM9DS1_DEV_NAME "lsm9ds1" + +#define ST_IMU68_OUT_LEN 6 + +#if defined(CONFIG_SPI_MASTER) +#define ST_IMU68_RX_MAX_LENGTH 8 +#define ST_IMU68_TX_MAX_LENGTH 8 + +struct st_imu68_transfer_buffer { + u8 rx_buf[ST_IMU68_RX_MAX_LENGTH]; + u8 tx_buf[ST_IMU68_TX_MAX_LENGTH] ____cacheline_aligned; +}; +#endif /* CONFIG_SPI_MASTER */ + +struct st_imu68_transfer_function { + int (*read)(struct device *dev, u8 addr, int len, u8 *data); + int (*write)(struct device *dev, u8 addr, int len, u8 *data); +}; + +enum st_imu68_sensor_id { + ST_IMU68_ID_ACC, + ST_IMU68_ID_GYRO, + ST_IMU68_ID_MAX, +}; + +struct st_imu68_reg { + u8 addr; + u8 mask; +}; + +struct st_imu68_sensor { + enum st_imu68_sensor_id id; + struct st_imu68_hw *hw; + + struct iio_trigger *trigger; + + u32 gain; + u16 odr; + + u8 drdy_mask; + u8 status_mask; +}; + +struct st_imu68_hw { + const char *name; + struct device *dev; + int irq; + + struct mutex lock; + + s64 timestamp; + u8 enabled_mask; + u32 module_id; + + struct iio_dev *iio_devs[ST_IMU68_ID_MAX]; + + const struct st_imu68_transfer_function *tf; +#if defined(CONFIG_SPI_MASTER) + struct st_imu68_transfer_buffer tb; +#endif /* CONFIG_SPI_MASTER */ +}; + +int st_imu68_write_with_mask(struct st_imu68_hw *hw, u8 addr, u8 mask, + u8 val); +int st_imu68_probe(struct device *dev, int irq, const char *name, + const struct st_imu68_transfer_function *tf_ops); +int st_imu68_remove(struct device *dev); +int st_imu68_sensor_enable(struct st_imu68_sensor *sensor, bool enable); +int st_imu68_allocate_buffers(struct st_imu68_hw *hw); +void st_imu68_deallocate_buffers(struct st_imu68_hw *hw); +int st_imu68_allocate_triggers(struct st_imu68_hw *hw); +void st_imu68_deallocate_triggers(struct st_imu68_hw *hw); +#endif /* ST_IMU68_H */ diff --git a/drivers/iio/stm/imu/st_imu68/st_imu68_buffer.c b/drivers/iio/stm/imu/st_imu68/st_imu68_buffer.c new file mode 100644 index 000000000000..2567c0e5c4d8 --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/st_imu68_buffer.c @@ -0,0 +1,200 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_imu68 buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_imu68.h" + +#define ST_IMU68_REG_INT1_CTRL_ADDR 0x0c +#define ST_IMU68_REG_INT2_CTRL_ADDR 0x0d +#define ST_IMU68_REG_STATUS_ADDR 0x17 + +static int st_imu68_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig); + struct st_imu68_sensor *sensor = iio_priv(iio_dev); + int err; + + err = st_imu68_write_with_mask(sensor->hw, ST_IMU68_REG_INT1_CTRL_ADDR, + sensor->drdy_mask, state); + + return err < 0 ? err : 0; +} + +static const struct iio_trigger_ops st_imu68_trigger_ops = { + .set_trigger_state = st_imu68_trig_set_state, +}; + +static irqreturn_t st_imu68_trigger_irq_handler(int irq, void *p) +{ + struct st_imu68_hw *hw = (struct st_imu68_hw *)p; + + hw->timestamp = iio_get_time_ns(hw->iio_devs[0]); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_imu68_trigger_thread_handler(int irq, void *p) +{ + struct st_imu68_hw *hw = (struct st_imu68_hw *)p; + struct st_imu68_sensor *sensor; + int i, err, count = 0; + u8 status; + + err = hw->tf->read(hw->dev, ST_IMU68_REG_STATUS_ADDR, sizeof(status), + &status); + if (err < 0) + return IRQ_HANDLED; + + for (i = 0; i < ST_IMU68_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + + if (status & sensor->status_mask) { + iio_trigger_poll_chained(sensor->trigger); + count++; + } + } + + return count > 0 ? IRQ_HANDLED : IRQ_NONE; +} + +int st_imu68_allocate_triggers(struct st_imu68_hw *hw) +{ + struct st_imu68_sensor *sensor; + int i, err; + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_imu68_trigger_irq_handler, + st_imu68_trigger_thread_handler, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + hw->name, hw); + if (err) + return err; + + for (i = 0; i < ST_IMU68_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + sensor->trigger = devm_iio_trigger_alloc(hw->dev, "%s-trigger", + hw->iio_devs[i]->name); + if (!sensor->trigger) { + dev_err(hw->dev, "failed to allocate iio trigger.\n"); + err = -ENOMEM; + goto err; + } + iio_trigger_set_drvdata(sensor->trigger, hw->iio_devs[i]); + sensor->trigger->ops = &st_imu68_trigger_ops; + sensor->trigger->dev.parent = hw->dev; + + err = iio_trigger_register(sensor->trigger); + if (err < 0) { + dev_err(hw->dev, "failed to register iio trigger.\n"); + + goto err; + } + hw->iio_devs[i]->trig = sensor->trigger; + } + + return 0; + +err: + for (i--; i >= 0; i--) { + sensor = iio_priv(hw->iio_devs[i]); + iio_trigger_unregister(sensor->trigger); + } + + return err; +} + +void st_imu68_deallocate_triggers(struct st_imu68_hw *hw) +{ + struct st_imu68_sensor *sensor; + int i; + + for (i = 0; i < ST_IMU68_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + iio_trigger_unregister(sensor->trigger); + } +} + +static int st_imu68_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_imu68_sensor_enable(iio_priv(iio_dev), true); +} + +static int st_imu68_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_imu68_sensor_enable(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_imu68_buffer_setup_ops = { + .preenable = st_imu68_buffer_preenable, +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0) + .postenable = iio_triggered_buffer_postenable, + .predisable = iio_triggered_buffer_predisable, +#endif /* LINUX_VERSION_CODE */ + .postdisable = st_imu68_buffer_postdisable, +}; + +static irqreturn_t st_imu68_buffer_thread_handler(int irq, void *p) +{ + u8 buffer[ALIGN(ST_IMU68_OUT_LEN, sizeof(s64)) + sizeof(s64)]; + struct iio_poll_func *pf = p; + struct iio_chan_spec const *ch = pf->indio_dev->channels; + struct st_imu68_sensor *sensor = iio_priv(pf->indio_dev); + struct st_imu68_hw *hw = sensor->hw; + int err; + + err = hw->tf->read(hw->dev, ch->address, ST_IMU68_OUT_LEN, buffer); + if (err < 0) + goto out; + + iio_push_to_buffers_with_timestamp(pf->indio_dev, buffer, + hw->timestamp); + +out: + iio_trigger_notify_done(sensor->trigger); + + return IRQ_HANDLED; +} + +int st_imu68_allocate_buffers(struct st_imu68_hw *hw) +{ + int err, i; + + for (i = 0; i < ST_IMU68_ID_MAX; i++) { + err = iio_triggered_buffer_setup(hw->iio_devs[i], NULL, + st_imu68_buffer_thread_handler, + &st_imu68_buffer_setup_ops); + if (err) + goto err; + } + + return 0; + +err: + for (i--; i >= 0; i--) + iio_triggered_buffer_cleanup(hw->iio_devs[i]); + + return err; +} + +void st_imu68_deallocate_buffers(struct st_imu68_hw *hw) +{ + int i; + + for (i = 0; i < ST_IMU68_ID_MAX; i++) + iio_triggered_buffer_cleanup(hw->iio_devs[i]); +} diff --git a/drivers/iio/stm/imu/st_imu68/st_imu68_core.c b/drivers/iio/stm/imu/st_imu68/st_imu68_core.c new file mode 100644 index 000000000000..3eed3cb15f6e --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/st_imu68_core.c @@ -0,0 +1,662 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_imu68 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2019 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_imu68.h" + +#define ST_IMU68_WHOAMI_ADDR 0x0f +#define ST_IMU68_WHOAMI 0x68 + +#define ST_IMU68_REG_GYRO_ODR_ADDR 0x10 +#define ST_IMU68_REG_GYRO_ODR_MASK GENMASK(7, 5) +#define ST_IMU68_REG_GYRO_FS_ADDR 0x10 +#define ST_IMU68_REG_GYRO_FS_MASK GENMASK(4, 3) +#define ST_IMU68_REG_CTRL4_ADDR 0x1e +#define ST_IMU68_GYRO_EN_MASK GENMASK(5, 3) +#define ST_IMU68_REG_CTRL5_ADDR 0x1f +#define ST_IMU68_ACC_EN_MASK GENMASK(5, 3) +#define ST_IMU68_REG_ACC_ODR_ADDR 0x20 +#define ST_IMU68_REG_ACC_ODR_MASK GENMASK(7, 5) +#define ST_IMU68_REG_ACC_FS_ADDR 0x20 +#define ST_IMU68_REG_ACC_FS_MASK GENMASK(4, 3) + +#define ST_IMU68_INT_ACC_DRDY_MASK BIT(0) +#define ST_IMU68_ACC_STATUS_MASK BIT(0) +#define ST_IMU68_INT_GYRO_DRDY_MASK BIT(1) +#define ST_IMU68_GYRO_STATUS_MASK BIT(1) + +#define ST_IMU68_REG_GYRO_OUT_X_L_ADDR 0x18 +#define ST_IMU68_REG_GYRO_OUT_Y_L_ADDR 0x1a +#define ST_IMU68_REG_GYRO_OUT_Z_L_ADDR 0x1c + +#define ST_IMU68_REG_ACC_OUT_X_L_ADDR 0x28 +#define ST_IMU68_REG_ACC_OUT_Y_L_ADDR 0x2a +#define ST_IMU68_REG_ACC_OUT_Z_L_ADDR 0x2c + +#define ST_IMU68_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_IMU68_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_IMU68_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_IMU68_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(732000) + +#define ST_IMU68_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_IMU68_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_IMU68_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) + +struct st_imu68_odr { + u16 hz; + u8 val; +}; + +#define ST_IMU68_ODR_LIST_SIZE 5 +struct st_imu68_odr_table_entry { + struct st_imu68_reg reg; + struct st_imu68_odr odr_avl[ST_IMU68_ODR_LIST_SIZE]; +}; + +static const struct st_imu68_odr_table_entry st_imu68_odr_table[] = { + [ST_IMU68_ID_ACC] = { + .reg = { + .addr = ST_IMU68_REG_ACC_ODR_ADDR, + .mask = ST_IMU68_REG_ACC_ODR_MASK, + }, + .odr_avl[0] = { 10, 0x01 }, + .odr_avl[1] = { 50, 0x02 }, + .odr_avl[2] = { 119, 0x03 }, + .odr_avl[3] = { 238, 0x04 }, + .odr_avl[4] = { 476, 0x05 }, + }, + [ST_IMU68_ID_GYRO] = { + .reg = { + .addr = ST_IMU68_REG_GYRO_ODR_ADDR, + .mask = ST_IMU68_REG_GYRO_ODR_MASK, + }, + .odr_avl[0] = { 15, 0x01 }, + .odr_avl[1] = { 60, 0x02 }, + .odr_avl[2] = { 119, 0x03 }, + .odr_avl[3] = { 238, 0x04 }, + .odr_avl[4] = { 476, 0x05 }, + } +}; + +struct st_imu68_fs { + u32 gain; + u8 val; +}; + +#define ST_IMU68_FS_LIST_SIZE 4 +struct st_imu68_fs_table_entry { + struct st_imu68_reg reg; + struct st_imu68_fs fs_avl[ST_IMU68_FS_LIST_SIZE]; + u8 depth; +}; + +static const struct st_imu68_fs_table_entry st_imu68_fs_table[] = { + [ST_IMU68_ID_ACC] = { + .reg = { + .addr = ST_IMU68_REG_ACC_FS_ADDR, + .mask = ST_IMU68_REG_ACC_FS_MASK, + }, + .fs_avl[0] = { ST_IMU68_ACC_FS_2G_GAIN, 0x0 }, + .fs_avl[1] = { ST_IMU68_ACC_FS_4G_GAIN, 0x2 }, + .fs_avl[2] = { ST_IMU68_ACC_FS_8G_GAIN, 0x3 }, + .fs_avl[3] = { ST_IMU68_ACC_FS_16G_GAIN, 0x1 }, + }, + [ST_IMU68_ID_GYRO] = { + .reg = { + .addr = ST_IMU68_REG_GYRO_FS_ADDR, + .mask = ST_IMU68_REG_GYRO_FS_MASK, + }, + .fs_avl[0] = { ST_IMU68_GYRO_FS_250_GAIN, 0x0 }, + .fs_avl[1] = { ST_IMU68_GYRO_FS_500_GAIN, 0x1 }, + .fs_avl[2] = { ST_IMU68_GYRO_FS_2000_GAIN, 0x3 }, + } +}; + +#define ST_IMU68_CHANNEL(chan_type, addr, mod, scan_idx) \ +{ \ + .type = chan_type, \ + .address = addr, \ + .modified = 1, \ + .channel2 = mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_imu68_acc_channels[] = { + ST_IMU68_CHANNEL(IIO_ACCEL, ST_IMU68_REG_ACC_OUT_X_L_ADDR, + IIO_MOD_X, 0), + ST_IMU68_CHANNEL(IIO_ACCEL, ST_IMU68_REG_ACC_OUT_Y_L_ADDR, + IIO_MOD_Y, 1), + ST_IMU68_CHANNEL(IIO_ACCEL, ST_IMU68_REG_ACC_OUT_Z_L_ADDR, + IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_imu68_gyro_channels[] = { + ST_IMU68_CHANNEL(IIO_ANGL_VEL, ST_IMU68_REG_GYRO_OUT_X_L_ADDR, + IIO_MOD_X, 0), + ST_IMU68_CHANNEL(IIO_ANGL_VEL, ST_IMU68_REG_GYRO_OUT_Y_L_ADDR, + IIO_MOD_Y, 1), + ST_IMU68_CHANNEL(IIO_ANGL_VEL, ST_IMU68_REG_GYRO_OUT_Z_L_ADDR, + IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static inline int st_imu68_claim_direct_mode(struct iio_dev *iio_dev) +{ + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + return 0; +} + +static inline void st_imu68_release_direct_mode(struct iio_dev *iio_dev) +{ + mutex_unlock(&iio_dev->mlock); +} + +int st_imu68_write_with_mask(struct st_imu68_hw *hw, u8 addr, u8 mask, u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + goto out; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); + if (err < 0) + dev_err(hw->dev, "failed to write %02x register\n", addr); + +out: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_imu68_check_whoami(struct st_imu68_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_IMU68_WHOAMI_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_IMU68_WHOAMI) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + return 0; +} + +static int st_imu68_set_full_scale(struct st_imu68_sensor *sensor, u32 gain) +{ + enum st_imu68_sensor_id id = sensor->id; + int i, err; + u8 val; + + for (i = 0; i < ST_IMU68_FS_LIST_SIZE; i++) + if ((st_imu68_fs_table[id].fs_avl[i].gain == gain) && + st_imu68_fs_table[id].fs_avl[i].gain) + break; + + if (i == ST_IMU68_FS_LIST_SIZE) + return -EINVAL; + + val = st_imu68_fs_table[id].fs_avl[i].val; + err = st_imu68_write_with_mask(sensor->hw, + st_imu68_fs_table[id].reg.addr, + st_imu68_fs_table[id].reg.mask, val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_imu68_set_odr(struct st_imu68_sensor *sensor, u16 odr) +{ + enum st_imu68_sensor_id id = sensor->id; + int i, err; + u8 val; + + for (i = 0; i < ST_IMU68_ODR_LIST_SIZE; i++) + if (st_imu68_odr_table[id].odr_avl[i].hz >= odr) + break; + + if (i == ST_IMU68_ODR_LIST_SIZE) + return -EINVAL; + + val = st_imu68_odr_table[id].odr_avl[i].val; + err = st_imu68_write_with_mask(sensor->hw, + st_imu68_odr_table[id].reg.addr, + st_imu68_odr_table[id].reg.mask, val); + if (err < 0) + return err; + + sensor->odr = st_imu68_odr_table[id].odr_avl[i].hz; + + return 0; +} + +static ssize_t +st_imu68_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_imu68_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_imu68_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < ST_IMU68_ODR_LIST_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_imu68_odr_table[id].odr_avl[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_imu68_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_imu68_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_imu68_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < ST_IMU68_FS_LIST_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_imu68_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static int st_imu68_read_oneshot(struct st_imu68_sensor *sensor, u8 addr, + int *val) +{ + int err, delay; + __le16 data; + + err = st_imu68_sensor_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data), + (u8 *)&data); + if (err < 0) + return err; + + st_imu68_sensor_enable(sensor, false); + + *val = (s16)data; + + return IIO_VAL_INT; +} + +static int st_imu68_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_imu68_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = st_imu68_claim_direct_mode(iio_dev); + if (ret) + break; + + ret = st_imu68_read_oneshot(sensor, ch->address, val); + st_imu68_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_imu68_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == IIO_CHAN_INFO_SCALE) { + if ((chan->type == IIO_ANGL_VEL) || + (chan->type == IIO_ACCEL)) + return IIO_VAL_INT_PLUS_NANO; + } + + return -EINVAL; +} + +static int st_imu68_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_imu68_sensor *sensor = iio_priv(iio_dev); + int err; + + err = st_imu68_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_imu68_set_full_scale(sensor, val2); + break; + default: + err = -EINVAL; + break; + } + + st_imu68_release_direct_mode(iio_dev); + + return err; +} + +int st_imu68_sensor_enable(struct st_imu68_sensor *sensor, bool enable) +{ + int err; + enum st_imu68_sensor_id id = sensor->id; + + if (enable) { + u16 odr; + struct st_imu68_sensor *sensor_acc = + iio_priv(sensor->hw->iio_devs[ST_IMU68_ID_ACC]); + + /* Check if Gyro enabling with Acc already on */ + if ((id == ST_IMU68_ID_GYRO) && + (sensor->hw->enabled_mask & BIT(ST_IMU68_ID_ACC))) { + odr = max(sensor->odr, sensor_acc->odr); + } else { + odr = sensor->odr; + } + + err = st_imu68_set_odr(sensor, odr); + if (err < 0) + return err; + + sensor->hw->enabled_mask |= BIT(id); + } else { + err = st_imu68_write_with_mask(sensor->hw, + st_imu68_odr_table[id].reg.addr, + st_imu68_odr_table[id].reg.mask, + 0); + if (err < 0) + return err; + + sensor->hw->enabled_mask &= ~BIT(id); + } + + return 0; +} + +static ssize_t st_imu68_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_imu68_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->odr); +} + +static ssize_t st_imu68_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct st_imu68_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err, odr; + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + err = st_imu68_set_odr(sensor, odr); + + return err < 0 ? err : count; +} + +ssize_t st_imu68_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_imu68_sensor *sensor = iio_priv(iio_dev); + struct st_imu68_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_imu68_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_imu68_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_imu68_sysfs_scale_avail, NULL, 0); +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_imu68_get_sampling_frequency, + st_imu68_set_sampling_frequency); +static IIO_DEVICE_ATTR(module_id, 0444, st_imu68_get_module_id, NULL, 0); + +static struct attribute *st_imu68_acc_attributes[] = { + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_imu68_acc_attribute_group = { + .attrs = st_imu68_acc_attributes, +}; + +static const struct iio_info st_imu68_acc_info = { + .attrs = &st_imu68_acc_attribute_group, + .read_raw = st_imu68_read_raw, + .write_raw = st_imu68_write_raw, + .write_raw_get_fmt = st_imu68_write_raw_get_fmt, +}; + +static struct attribute *st_imu68_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_imu68_gyro_attribute_group = { + .attrs = st_imu68_gyro_attributes, +}; + +static const struct iio_info st_imu68_gyro_info = { + .attrs = &st_imu68_gyro_attribute_group, + .read_raw = st_imu68_read_raw, + .write_raw = st_imu68_write_raw, + .write_raw_get_fmt = st_imu68_write_raw_get_fmt, +}; + +static const unsigned long st_imu68_available_scan_masks[] = {0x7, 0x0}; + +static struct iio_dev *st_imu68_alloc_iiodev(struct st_imu68_hw *hw, + enum st_imu68_sensor_id id) +{ + struct st_imu68_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; + iio_dev->available_scan_masks = st_imu68_available_scan_masks; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = st_imu68_odr_table[id].odr_avl[0].hz; + sensor->gain = st_imu68_fs_table[id].fs_avl[0].gain; + + switch (id) { + case ST_IMU68_ID_ACC: + iio_dev->channels = st_imu68_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_imu68_acc_channels); + iio_dev->name = kasprintf(GFP_KERNEL, "%s_accel", hw->name); + iio_dev->info = &st_imu68_acc_info; + + sensor->drdy_mask = ST_IMU68_INT_ACC_DRDY_MASK; + sensor->status_mask = ST_IMU68_ACC_STATUS_MASK; + break; + case ST_IMU68_ID_GYRO: + iio_dev->channels = st_imu68_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_imu68_gyro_channels); + iio_dev->name = kasprintf(GFP_KERNEL, "%s_gyro", hw->name); + iio_dev->info = &st_imu68_gyro_info; + + sensor->drdy_mask = ST_IMU68_INT_GYRO_DRDY_MASK; + sensor->status_mask = ST_IMU68_GYRO_STATUS_MASK; + break; + default: + return NULL; + } + + return iio_dev; +} + +static void st_imu68_get_properties(struct st_imu68_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +int st_imu68_probe(struct device *dev, int irq, const char *name, + const struct st_imu68_transfer_function *tf_ops) +{ + struct st_imu68_hw *hw; + int err, i; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + + mutex_init(&hw->lock); + hw->name = name; + hw->dev = dev; + hw->irq = irq; + hw->tf = tf_ops; + hw->enabled_mask = 0; + + err = st_imu68_check_whoami(hw); + if (err < 0) + return err; + + st_imu68_get_properties(hw); + + err = st_imu68_write_with_mask(hw, ST_IMU68_REG_CTRL4_ADDR, + ST_IMU68_GYRO_EN_MASK, 0x7); + if (err < 0) + return err; + + err = st_imu68_write_with_mask(hw, ST_IMU68_REG_CTRL5_ADDR, + ST_IMU68_ACC_EN_MASK, 0x7); + if (err < 0) + return err; + + for (i = 0; i < ST_IMU68_ID_MAX; i++) { + hw->iio_devs[i] = st_imu68_alloc_iiodev(hw, i); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + if (irq > 0) { + err = st_imu68_allocate_buffers(hw); + if (err) + return err; + + err = st_imu68_allocate_triggers(hw); + if (err) + goto deallocate_iio_buffers; + } + + for (i = 0; i < ST_IMU68_ID_MAX; i++) { + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + goto deallocate_iio_triggers; + } + + return 0; + +deallocate_iio_triggers: + if (irq > 0) + st_imu68_deallocate_triggers(hw); +deallocate_iio_buffers: + if (irq > 0) + st_imu68_deallocate_buffers(hw); + + return err; +} +EXPORT_SYMBOL(st_imu68_probe); + +int st_imu68_remove(struct device *dev) +{ + struct st_imu68_hw *hw = dev_get_drvdata(dev); + + if (hw->irq) { + st_imu68_deallocate_triggers(hw); + st_imu68_deallocate_buffers(hw); + } + + return 0; +} +EXPORT_SYMBOL(st_imu68_remove); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_imu68 driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_imu68/st_imu68_i2c.c b/drivers/iio/stm/imu/st_imu68/st_imu68_i2c.c new file mode 100644 index 000000000000..5f08b3e470e6 --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/st_imu68_i2c.c @@ -0,0 +1,76 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_imu68 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_imu68.h" + +static int st_imu68_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + return i2c_smbus_read_i2c_block_data_or_emulated(to_i2c_client(dev), + addr, len, data); +} + +static int st_imu68_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + return i2c_smbus_write_i2c_block_data(to_i2c_client(dev), addr, len, + data); +} + +static const struct st_imu68_transfer_function st_imu68_transfer_fn = { + .read = st_imu68_i2c_read, + .write = st_imu68_i2c_write, +}; + +static int st_imu68_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_imu68_probe(&client->dev, client->irq, client->name, + &st_imu68_transfer_fn); +} + +static int st_imu68_i2c_remove(struct i2c_client *client) +{ + st_imu68_remove(&client->dev); + + return 0; +} + +static const struct of_device_id st_imu68_i2c_of_match[] = { + { + .compatible = "st,lsm9ds1", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_imu68_i2c_of_match); + +static const struct i2c_device_id st_imu68_i2c_id_table[] = { + { ST_LSM9DS1_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_imu68_i2c_id_table); + +static struct i2c_driver st_imu68_driver = { + .driver = { + .name = "st_imu68_i2c", + .of_match_table = of_match_ptr(st_imu68_i2c_of_match), + }, + .probe = st_imu68_i2c_probe, + .remove = st_imu68_i2c_remove, + .id_table = st_imu68_i2c_id_table, +}; +module_i2c_driver(st_imu68_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_imu68 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_imu68/st_imu68_spi.c b/drivers/iio/stm/imu/st_imu68/st_imu68_spi.c new file mode 100644 index 000000000000..6b8c546ccb22 --- /dev/null +++ b/drivers/iio/stm/imu/st_imu68/st_imu68_spi.c @@ -0,0 +1,112 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_imu68 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_imu68.h" + +#define SENSORS_SPI_READ BIT(7) + +static int st_imu68_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_imu68_hw *hw = spi_get_drvdata(spi); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_imu68_spi_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_imu68_hw *hw; + struct spi_device *spi; + + if (len >= ST_IMU68_TX_MAX_LENGTH) + return -ENOMEM; + + spi = to_spi_device(dev); + hw = spi_get_drvdata(spi); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_imu68_transfer_function st_imu68_transfer_fn = { + .read = st_imu68_spi_read, + .write = st_imu68_spi_write, +}; + +static int st_imu68_spi_probe(struct spi_device *spi) +{ + return st_imu68_probe(&spi->dev, spi->irq, spi->modalias, + &st_imu68_transfer_fn); +} + +static int st_imu68_spi_remove(struct spi_device *spi) +{ + st_imu68_remove(&spi->dev); + + return 0; +} + +static const struct of_device_id st_imu68_spi_of_match[] = { + { + .compatible = "st,lsm9ds1", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_imu68_spi_of_match); + +static const struct spi_device_id st_imu68_spi_id_table[] = { + { ST_LSM9DS1_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_imu68_spi_id_table); + +static struct spi_driver st_imu68_driver = { + .driver = { + .name = "st_imu68_spi", + .of_match_table = of_match_ptr(st_imu68_spi_of_match), + }, + .probe = st_imu68_spi_probe, + .remove = st_imu68_spi_remove, + .id_table = st_imu68_spi_id_table, +}; +module_spi_driver(st_imu68_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_imu68 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dhcx/Kconfig b/drivers/iio/stm/imu/st_ism330dhcx/Kconfig new file mode 100644 index 000000000000..da3123bfa9b3 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/Kconfig @@ -0,0 +1,22 @@ +# SPDX-License-Identifier: GPL-2.0-only +config IIO_ST_ISM330DHCX + tristate "STMicroelectronics ISM330DHCX sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_ISM330DHCX_I2C if (I2C) + select IIO_ST_ISM330DHCX_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics ISM330DHCX imu + sensor. + + To compile this driver as a module, choose M here: the module + will be called st_ism330dhcx. + +config IIO_ST_ISM330DHCX_I2C + tristate + depends on IIO_ST_ISM330DHCX + +config IIO_ST_ISM330DHCX_SPI + tristate + depends on IIO_ST_ISM330DHCX diff --git a/drivers/iio/stm/imu/st_ism330dhcx/Makefile b/drivers/iio/stm/imu/st_ism330dhcx/Makefile new file mode 100644 index 000000000000..dc97485348ce --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/Makefile @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_ism330dhcx-y := st_ism330dhcx_core.o st_ism330dhcx_buffer.o \ + st_ism330dhcx_shub.o st_ism330dhcx_embfunc.o + +obj-$(CONFIG_IIO_ST_ISM330DHCX) += st_ism330dhcx.o +obj-$(CONFIG_IIO_ST_ISM330DHCX_I2C) += st_ism330dhcx_i2c.o +obj-$(CONFIG_IIO_ST_ISM330DHCX_SPI) += st_ism330dhcx_spi.o diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx.h b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx.h new file mode 100644 index 000000000000..b6fc5d4ee923 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx.h @@ -0,0 +1,582 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_ism330dhcx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#ifndef ST_ISM330DHCX_H +#define ST_ISM330DHCX_H + +#include +#include +#include + +#define ST_ISM330DHCX_MAX_ODR 833 +#define ST_ISM330DHCX_ODR_LIST_SIZE 8 +#define ST_ISM330DHCX_ODR_EXPAND(odr, uodr) ((odr * 1000000) + uodr) + +#define ST_ISM330DHCX_DEV_NAME "ism330dhcx" + +#define ST_ISM330DHCX_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_ISM330DHCX_REG_SHUB_REG_MASK BIT(6) +#define ST_ISM330DHCX_REG_FUNC_CFG_MASK BIT(7) + +#define ST_ISM330DHCX_REG_FIFO_CTRL1_ADDR 0x07 +#define ST_ISM330DHCX_REG_FIFO_CTRL2_ADDR 0x08 +#define ST_ISM330DHCX_REG_FIFO_WTM_MASK GENMASK(8, 0) +#define ST_ISM330DHCX_REG_FIFO_WTM8_MASK BIT(0) + +#define ST_ISM330DHCX_REG_FIFO_CTRL3_ADDR 0x09 +#define ST_ISM330DHCX_REG_BDR_XL_MASK GENMASK(3, 0) +#define ST_ISM330DHCX_REG_BDR_GY_MASK GENMASK(7, 4) + +#define ST_ISM330DHCX_REG_FIFO_CTRL4_ADDR 0x0a +#define ST_ISM330DHCX_REG_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_ISM330DHCX_REG_ODR_T_BATCH_MASK GENMASK(5, 4) +#define ST_ISM330DHCX_REG_DEC_TS_MASK GENMASK(7, 6) + +#define ST_ISM330DHCX_REG_INT1_CTRL_ADDR 0x0d +#define ST_ISM330DHCX_REG_INT2_CTRL_ADDR 0x0e +#define ST_ISM330DHCX_REG_INT_FIFO_TH_MASK BIT(3) + +#define ST_ISM330DHCX_REG_WHOAMI_ADDR 0x0f +#define ST_ISM330DHCX_WHOAMI_VAL 0x6b + +#define ST_ISM330DHCX_CTRL1_XL_ADDR 0x10 +#define ST_ISM330DHCX_CTRL2_G_ADDR 0x11 +#define ST_ISM330DHCX_REG_CTRL3_C_ADDR 0x12 +#define ST_ISM330DHCX_REG_SW_RESET_MASK BIT(0) +#define ST_ISM330DHCX_REG_PP_OD_MASK BIT(4) +#define ST_ISM330DHCX_REG_H_LACTIVE_MASK BIT(5) +#define ST_ISM330DHCX_REG_BDU_MASK BIT(6) +#define ST_ISM330DHCX_REG_BOOT_MASK BIT(7) + +#define ST_ISM330DHCX_REG_CTRL4_C_ADDR 0x13 +#define ST_ISM330DHCX_REG_DRDY_MASK BIT(3) + +#define ST_ISM330DHCX_REG_CTRL5_C_ADDR 0x14 +#define ST_ISM330DHCX_REG_ROUNDING_MASK GENMASK(6, 5) + +#define ST_ISM330DHCX_REG_CTRL9_XL_ADDR 0x18 +#define ST_ISM330DHCX_REG_I3C_DISABLE_MASK BIT(1) + +#define ST_ISM330DHCX_REG_CTRL10_C_ADDR 0x19 +#define ST_ISM330DHCX_REG_TIMESTAMP_EN_MASK BIT(5) + +#define ST_ISM330DHCX_REG_OUT_TEMP_L_ADDR 0x20 + +#define ST_ISM330DHCX_REG_OUTX_L_G_ADDR 0x22 +#define ST_ISM330DHCX_REG_OUTY_L_G_ADDR 0x24 +#define ST_ISM330DHCX_REG_OUTZ_L_G_ADDR 0x26 + +#define ST_ISM330DHCX_REG_OUTX_L_A_ADDR 0x28 +#define ST_ISM330DHCX_REG_OUTY_L_A_ADDR 0x2a +#define ST_ISM330DHCX_REG_OUTZ_L_A_ADDR 0x2c + +#define ST_ISM330DHCX_REG_FIFO_STATUS1_ADDR 0x3a +#define ST_ISM330DHCX_REG_FIFO_STATUS_DIFF GENMASK(9, 0) + +#define ST_ISM330DHCX_REG_TIMESTAMP0_ADDR 0x40 +#define ST_ISM330DHCX_REG_TIMESTAMP2_ADDR 0x42 + +#define ST_ISM330DHCX_REG_TAP_CFG0_ADDR 0x56 +#define ST_ISM330DHCX_REG_TAP_X_EN_MASK BIT(3) +#define ST_ISM330DHCX_REG_TAP_Y_EN_MASK BIT(2) +#define ST_ISM330DHCX_REG_TAP_Z_EN_MASK BIT(1) +#define ST_ISM330DHCX_REG_LIR_MASK BIT(0) + +#define ST_ISM330DHCX_REG_MD1_CFG_ADDR 0x5e +#define ST_ISM330DHCX_REG_MD2_CFG_ADDR 0x5f +#define ST_ISM330DHCX_REG_INT2_TIMESTAMP_MASK BIT(0) +#define ST_ISM330DHCX_REG_INT_EMB_FUNC_MASK BIT(1) + +#define ST_ISM330DHCX_INTERNAL_FREQ_FINE 0x63 + +#define ST_ISM330DHCX_REG_FIFO_DATA_OUT_TAG_ADDR 0x78 + +/* embedded registers */ +#define ST_ISM330DHCX_REG_EMB_FUNC_INT1_ADDR 0x0a +#define ST_ISM330DHCX_REG_EMB_FUNC_INT2_ADDR 0x0e + +/* Timestamp Tick 25us/LSB */ +#define ST_ISM330DHCX_TS_DELTA_NS 25000ULL + +#define ST_ISM330DHCX_TEMP_GAIN 256 +#define ST_ISM330DHCX_TEMP_FS_GAIN (1000000 / ST_ISM330DHCX_TEMP_GAIN) +#define ST_ISM330DHCX_TEMP_OFFSET 6400 + +#define ST_ISM330DHCX_SAMPLE_SIZE 6 +#define ST_ISM330DHCX_TS_SAMPLE_SIZE 4 +#define ST_ISM330DHCX_TAG_SIZE 1 +#define ST_ISM330DHCX_FIFO_SAMPLE_SIZE (ST_ISM330DHCX_SAMPLE_SIZE + \ + ST_ISM330DHCX_TAG_SIZE) +#define ST_ISM330DHCX_MAX_FIFO_DEPTH 416 + +#define ST_ISM330DHCX_DATA_CHANNEL(chan_type, addr, mod, ch2, scan_idx, \ + rb, sb, sg) \ +{ \ + .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, \ + }, \ +} + +static const struct iio_event_spec st_ism330dhcx_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_ism330dhcx_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_ISM330DHCX_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_ism330dhcx_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_ISM330DHCX_RX_MAX_LENGTH 64 +#define ST_ISM330DHCX_TX_MAX_LENGTH 16 + +/** + * @struct st_ism330dhcx_transfer_buffer + * @brief Buffer support for data transfer + * + * rx_buf: Data receive buffer. + * tx_buf: Data transmit buffer. + */ +struct st_ism330dhcx_transfer_buffer { + u8 rx_buf[ST_ISM330DHCX_RX_MAX_LENGTH]; + u8 tx_buf[ST_ISM330DHCX_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +/** + * @struct st_ism330dhcx_transfer_function + * @brief Bus Transfer Function + * + * read: Bus read funtion to get register value from sensor. + * write: Bus write funtion to set register value to sensor. + */ +struct st_ism330dhcx_transfer_function { + int (*read)(struct device *dev, u8 addr, int len, u8 *data); + int (*write)(struct device *dev, u8 addr, int len, const u8 *data); +}; + +/** + * @struct st_ism330dhcx_reg + * @brief Generic sensor register description + * + * addr: Register arress value. + * mask: Register bitmask. + */ +struct st_ism330dhcx_reg { + u8 addr; + u8 mask; +}; + +enum st_ism330dhcx_suspend_resume_register { + ST_ISM330DHCX_CTRL1_XL_REG = 0, + ST_ISM330DHCX_CTRL2_G_REG, + ST_ISM330DHCX_REG_CTRL3_C_REG, + ST_ISM330DHCX_REG_CTRL4_C_REG, + ST_ISM330DHCX_REG_CTRL5_C_REG, + ST_ISM330DHCX_REG_CTRL10_C_REG, + ST_ISM330DHCX_REG_TAP_CFG0_REG, + ST_ISM330DHCX_REG_INT1_CTRL_REG, + ST_ISM330DHCX_REG_INT2_CTRL_REG, + ST_ISM330DHCX_REG_FIFO_CTRL1_REG, + ST_ISM330DHCX_REG_FIFO_CTRL2_REG, + ST_ISM330DHCX_REG_FIFO_CTRL3_REG, + ST_ISM330DHCX_REG_FIFO_CTRL4_REG, + ST_ISM330DHCX_SUSPEND_RESUME_REGS, +}; + +struct st_ism330dhcx_suspend_resume_entry { + u8 addr; + u8 val; + u8 mask; +}; + +/** + * @struct st_ism330dhcx_odr + * @brief ODR sensor table entry + * + * In the ODR table the possible ODR supported by sensor can be defined in the + * following format: + * .odr_avl[0] = { 0, 0, 0x00 }, + * .odr_avl[1] = { 12, 500000, 0x01 }, ..... it means 12.5 Hz + * .odr_avl[2] = { 26, 0, 0x02 }, ..... it means 26.0 Hz + * + * hz: Most significant part of ODR value (in Hz). + * uhz: Least significant part of ODR value (in micro Hz). + * val: Register value tu set ODR. + */ +struct st_ism330dhcx_odr { + int hz; + int uhz; + u8 val; +}; + +/** + * @struct st_ism330dhcx_odr_table_entry + * @brief ODR sensor table + * + * odr_size: ODR table size. + * reg: Sensor register description for ODR (address and mask). + * odr_avl: All supported ODR values. + */ +struct st_ism330dhcx_odr_table_entry { + u8 odr_size; + struct st_ism330dhcx_reg reg; + struct st_ism330dhcx_odr odr_avl[ST_ISM330DHCX_ODR_LIST_SIZE]; +}; + +/** + * @struct st_ism330dhcx_fs + * @brief Full scale entry + * + * reg: Sensor register description for FS (address and mask). + * gain: The gain to obtain data value from raw data (LSB). + * val: Register value. + */ +struct st_ism330dhcx_fs { + struct st_ism330dhcx_reg reg; + u32 gain; + u8 val; +}; + +/** + * @struct st_ism330dhcx_fs_table_entry + * @brief Full scale table + * + * size: Full scale number of entry. + * fs_avl: Full scale entry. + */ +#define ST_ISM330DHCX_FS_LIST_SIZE 5 +#define ST_ISM330DHCX_FS_ACC_LIST_SIZE 4 +#define ST_ISM330DHCX_FS_GYRO_LIST_SIZE 5 +#define ST_ISM330DHCX_FS_TEMP_LIST_SIZE 1 +struct st_ism330dhcx_fs_table_entry { + u8 size; + struct st_ism330dhcx_fs fs_avl[ST_ISM330DHCX_FS_LIST_SIZE]; +}; + +#define ST_ISM330DHCX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_ISM330DHCX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_ISM330DHCX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_ISM330DHCX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488000) + +#define ST_ISM330DHCX_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_ISM330DHCX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_ISM330DHCX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_ISM330DHCX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_ISM330DHCX_GYRO_FS_4000_GAIN IIO_DEGREE_TO_RAD(140000000) + +struct st_ism330dhcx_ext_dev_info { + const struct st_ism330dhcx_ext_dev_settings *ext_dev_settings; + u8 ext_dev_i2c_addr; +}; + +/** + * @enum st_ism330dhcx_sensor_id + * @brief Sensor Identifier + */ +enum st_ism330dhcx_sensor_id { + ST_ISM330DHCX_ID_GYRO, + ST_ISM330DHCX_ID_ACC, + ST_ISM330DHCX_ID_TEMP, + ST_ISM330DHCX_ID_EXT0, + ST_ISM330DHCX_ID_EXT1, + ST_ISM330DHCX_ID_STEP_COUNTER, + ST_ISM330DHCX_ID_STEP_DETECTOR, + ST_ISM330DHCX_ID_SIGN_MOTION, + ST_ISM330DHCX_ID_GLANCE, + ST_ISM330DHCX_ID_MOTION, + ST_ISM330DHCX_ID_NO_MOTION, + ST_ISM330DHCX_ID_WAKEUP, + ST_ISM330DHCX_ID_PICKUP, + ST_ISM330DHCX_ID_ORIENTATION, + ST_ISM330DHCX_ID_WRIST_TILT, + ST_ISM330DHCX_ID_TILT, + ST_ISM330DHCX_ID_MAX, +}; + +/** + * @enum st_ism330dhcx_sensor_id + * @brief Sensor Table Identifier + */ +static const enum st_ism330dhcx_sensor_id st_ism330dhcx_main_sensor_list[] = { + [0] = ST_ISM330DHCX_ID_GYRO, + [1] = ST_ISM330DHCX_ID_ACC, + [2] = ST_ISM330DHCX_ID_TEMP, + [3] = ST_ISM330DHCX_ID_STEP_COUNTER, + [4] = ST_ISM330DHCX_ID_STEP_DETECTOR, + [5] = ST_ISM330DHCX_ID_SIGN_MOTION, + [6] = ST_ISM330DHCX_ID_GLANCE, + [7] = ST_ISM330DHCX_ID_MOTION, + [8] = ST_ISM330DHCX_ID_NO_MOTION, + [9] = ST_ISM330DHCX_ID_WAKEUP, + [10] = ST_ISM330DHCX_ID_PICKUP, + [11] = ST_ISM330DHCX_ID_ORIENTATION, + [12] = ST_ISM330DHCX_ID_WRIST_TILT, + [13] = ST_ISM330DHCX_ID_TILT, +}; + +/** + * @enum st_ism330dhcx_fifo_mode + * @brief FIFO Modes + */ +enum st_ism330dhcx_fifo_mode { + ST_ISM330DHCX_FIFO_BYPASS = 0x0, + ST_ISM330DHCX_FIFO_CONT = 0x6, +}; + +/** + * @enum st_ism330dhcx_fifo_mode - FIFO Buffer Status + */ +enum st_ism330dhcx_fifo_status { + ST_ISM330DHCX_HW_FLUSH, + ST_ISM330DHCX_HW_OPERATIONAL, +}; + +/** + * @struct st_ism330dhcx_sensor + * @brief ST IMU sensor instance + * + * id: Sensor identifier + * hw: Pointer to instance of struct st_ism330dhcx_hw + * ext_dev_info: Sensor hub i2c slave settings. + * trig: Sensor iio trigger. + * gain: Configured sensor sensitivity + * odr: Output data rate of the sensor [Hz] + * uodr: Output data rate of the sensor [uHz] + * offset: Sensor data offset + * decimator: Sensor decimator + * dec_counter: Sensor decimator counter + * old_data: Saved sensor data + * max_watermark: Max supported watermark level + * watermark: Sensor watermark level + * batch_reg: Sensor reg/mask for FIFO batching register + * last_fifo_timestamp: Store last sample timestamp in FIFO, used by flush + */ +struct st_ism330dhcx_sensor { + enum st_ism330dhcx_sensor_id id; + struct st_ism330dhcx_hw *hw; + + struct st_ism330dhcx_ext_dev_info ext_dev_info; + + struct iio_trigger *trig; + + u32 gain; + int odr; + int uodr; + + u32 offset; + u8 decimator; + u8 dec_counter; + + u16 max_watermark; + u16 watermark; + + struct st_ism330dhcx_reg batch_reg; + s64 last_fifo_timestamp; +}; + +/** + * @struct st_ism330dhcx_hw + * @brief ST IMU MEMS hw instance + * + * dev: Pointer to instance of struct device (I2C or SPI). + * irq: Device interrupt line (I2C or SPI). + * 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. + * fsm_enable_mask: FSM Enabled sensor bitmask. + * embfunc_pg0_irq_reg: Embedded function irq configutation register (page 0). + * embfunc_irq_reg: Embedded function irq configutation register (other). + * ext_data_len: Number of i2c slave devices connected to I2C master. + * odr: Timestamp sample ODR [Hz] + * uodr: Timestamp sample ODR [uHz] + * ts_offset: Hw timestamp offset. + * hw_ts: Latest hw timestamp from the sensor. + * hw_ts_high: Manage timestamp rollover + * tsample: + * hw_ts_old: + * delta_ts: Delta time between two consecutive interrupts. + * delta_hw_ts: + * ts: Latest timestamp from irq handler. + * @module_id: identify iio devices of the same sensor module. + * iio_devs: Pointers to acc/gyro iio_dev instances. + * tf: Transfer function structure used by I/O operations. + * tb: Transfer buffers used by SPI I/O operations. + */ +struct st_ism330dhcx_hw { + struct device *dev; + int irq; + + struct mutex lock; + struct mutex fifo_lock; + struct mutex page_lock; + + enum st_ism330dhcx_fifo_mode fifo_mode; + unsigned long state; + u32 enable_mask; + u32 requested_mask; + + u16 fsm_enable_mask; + u8 embfunc_irq_reg; + u8 embfunc_pg0_irq_reg; + + u8 ext_data_len; + + int odr; + int uodr; + + 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_dev *iio_devs[ST_ISM330DHCX_ID_MAX]; + + const struct st_ism330dhcx_transfer_function *tf; + struct st_ism330dhcx_transfer_buffer tb; +}; + +/** + * @struct dev_pm_ops + * @brief Power mamagement callback function structure + */ +extern const struct dev_pm_ops st_ism330dhcx_pm_ops; + +static inline int st_ism330dhcx_read_atomic(struct st_ism330dhcx_hw *hw, u8 addr, + int len, u8 *data) +{ + int err; + + mutex_lock(&hw->page_lock); + err = hw->tf->read(hw->dev, addr, len, data); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int st_ism330dhcx_write_atomic(struct st_ism330dhcx_hw *hw, u8 addr, + int len, u8 *data) +{ + int err; + + mutex_lock(&hw->page_lock); + err = hw->tf->write(hw->dev, addr, len, data); + mutex_unlock(&hw->page_lock); + + return err; +} + +int __st_ism330dhcx_write_with_mask(struct st_ism330dhcx_hw *hw, u8 addr, u8 mask, + u8 val); +static inline int st_ism330dhcx_write_with_mask(struct st_ism330dhcx_hw *hw, u8 addr, + u8 mask, u8 val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_ism330dhcx_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int st_ism330dhcx_set_page_access(struct st_ism330dhcx_hw *hw, + u8 mask, u8 data) +{ + int err; + + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_FUNC_CFG_ACCESS_ADDR, + mask, data); + usleep_range(100, 150); + + return err; +} + +static inline bool st_ism330dhcx_is_fifo_enabled(struct st_ism330dhcx_hw *hw) +{ + return hw->enable_mask & (BIT(ST_ISM330DHCX_ID_STEP_COUNTER) | + BIT(ST_ISM330DHCX_ID_GYRO) | + BIT(ST_ISM330DHCX_ID_ACC) | + BIT(ST_ISM330DHCX_ID_EXT0) | + BIT(ST_ISM330DHCX_ID_EXT1)); +} + +int st_ism330dhcx_probe(struct device *dev, int irq, + const struct st_ism330dhcx_transfer_function *tf_ops); +int st_ism330dhcx_shub_set_enable(struct st_ism330dhcx_sensor *sensor, bool enable); +int st_ism330dhcx_shub_probe(struct st_ism330dhcx_hw *hw); +int st_ism330dhcx_sensor_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable); +int st_ism330dhcx_buffers_setup(struct st_ism330dhcx_hw *hw); +int st_ism330dhcx_get_odr_val(enum st_ism330dhcx_sensor_id id, int odr, int uodr, + int *podr, int *puodr, u8 *val); +int st_ism330dhcx_update_watermark(struct st_ism330dhcx_sensor *sensor, + u16 watermark); +ssize_t st_ism330dhcx_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_ism330dhcx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_ism330dhcx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_ism330dhcx_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_ism330dhcx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +int st_ism330dhcx_set_page_access(struct st_ism330dhcx_hw *hw, u8 mask, u8 data); +int st_ism330dhcx_suspend_fifo(struct st_ism330dhcx_hw *hw); +int st_ism330dhcx_set_fifo_mode(struct st_ism330dhcx_hw *hw, + enum st_ism330dhcx_fifo_mode fifo_mode); +int __st_ism330dhcx_set_sensor_batching_odr(struct st_ism330dhcx_sensor *sensor, + bool enable); +int st_ism330dhcx_fsm_init(struct st_ism330dhcx_hw *hw); +int st_ism330dhcx_fsm_get_orientation(struct st_ism330dhcx_hw *hw, u8 *data); +int st_ism330dhcx_embfunc_sensor_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable); +int st_ism330dhcx_step_counter_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable); +int st_ism330dhcx_reset_step_counter(struct iio_dev *iio_dev); +int st_ism330dhcx_update_batching(struct iio_dev *iio_dev, bool enable); +int st_ism330dhcx_reset_hwts(struct st_ism330dhcx_hw *hw); +#endif /* ST_ISM330DHCX_H */ diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_buffer.c b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_buffer.c new file mode 100644 index 000000000000..8f6672ee4e62 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_buffer.c @@ -0,0 +1,1023 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_ism330dhcx FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_ism330dhcx.h" + +#define ST_ISM330DHCX_REG_EMB_FUNC_STATUS_MAINPAGE 0x35 +#define ST_ISM330DHCX_REG_INT_STEP_DET_MASK BIT(3) +#define ST_ISM330DHCX_REG_INT_TILT_MASK BIT(4) +#define ST_ISM330DHCX_REG_INT_SIGMOT_MASK BIT(5) +#define ST_ISM330DHCX_REG_INT_GLANCE_MASK BIT(0) +#define ST_ISM330DHCX_REG_INT_MOTION_MASK BIT(1) +#define ST_ISM330DHCX_REG_INT_NO_MOTION_MASK BIT(2) +#define ST_ISM330DHCX_REG_INT_WAKEUP_MASK BIT(3) +#define ST_ISM330DHCX_REG_INT_PICKUP_MASK BIT(4) +#define ST_ISM330DHCX_REG_INT_ORIENTATION_MASK BIT(5) +#define ST_ISM330DHCX_REG_INT_WRIST_MASK BIT(6) + +#define ST_ISM330DHCX_SAMPLE_DISCHARD 0x7ffd + +#define ST_ISM330DHCX_EWMA_LEVEL 120 +#define ST_ISM330DHCX_EWMA_DIV 128 + +enum { + ST_ISM330DHCX_GYRO_TAG = 0x01, + ST_ISM330DHCX_ACC_TAG = 0x02, + ST_ISM330DHCX_TEMP_TAG = 0x03, + ST_ISM330DHCX_TS_TAG = 0x04, + ST_ISM330DHCX_EXT0_TAG = 0x0f, + ST_ISM330DHCX_EXT1_TAG = 0x10, + ST_ISM330DHCX_SC_TAG = 0x12, +}; + +/** + * Get Linux timestamp (SW) + * + * @return timestamp in ns + */ +static inline s64 st_ism330dhcx_get_time_ns(struct st_ism330dhcx_hw *hw) +{ + return iio_get_time_ns(hw->iio_devs[ST_ISM330DHCX_ID_GYRO]); +} + +/** + * Timestamp low pass filter + * + * @param old: ST IMU MEMS hw instance + * @param new: ST IMU MEMS hw instance + * @param weight: ST IMU MEMS hw instance + * @return estimation of the timestamp average + */ +static inline s64 st_ism330dhcx_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_ISM330DHCX_EWMA_DIV - weight) * diff, + ST_ISM330DHCX_EWMA_DIV); + + return old + incr; +} + +/** + * Reset HW Timestamp counter and clear timestamp data structure + * + * @param hw: ST IMU MEMS hw instance + * @return < 0 if error, 0 otherwise + */ +inline int st_ism330dhcx_reset_hwts(struct st_ism330dhcx_hw *hw) +{ + u8 data = 0xaa; + + hw->ts = st_ism330dhcx_get_time_ns(hw); + hw->ts_offset = hw->ts; + hw->val_ts_old = 0; + hw->hw_ts_high = 0; + hw->tsample = 0ull; + + return st_ism330dhcx_write_atomic(hw, ST_ISM330DHCX_REG_TIMESTAMP2_ADDR, + sizeof(data), &data); +} + +/** + * Setting FIFO mode + * + * @param hw: ST IMU MEMS hw instance + * @param fifo_mode: ST_ISM330DHCX_FIFO_BYPASS or ST_ISM330DHCX_FIFO_CONT + * @return 0 FIFO configured accordingly, non zero otherwise + */ +int st_ism330dhcx_set_fifo_mode(struct st_ism330dhcx_hw *hw, + enum st_ism330dhcx_fifo_mode fifo_mode) +{ + int err; + + err = st_ism330dhcx_write_with_mask(hw, ST_ISM330DHCX_REG_FIFO_CTRL4_ADDR, + ST_ISM330DHCX_REG_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +/** + * Setting sensor ODR in batching mode + * + * @param sensor: ST IMU sensor instance + * @param enable: enable or disable batching mode + * @return 0 FIFO configured accordingly, non zero otherwise + */ +int __st_ism330dhcx_set_sensor_batching_odr(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + u8 data = 0; + int err; + int podr, puodr; + + if (enable) { + err = st_ism330dhcx_get_odr_val(sensor->id, sensor->odr, + sensor->uodr, &podr, &puodr, + &data); + if (err < 0) + return err; + } + + err = __st_ism330dhcx_write_with_mask(hw, sensor->batch_reg.addr, + sensor->batch_reg.mask, data); + return err < 0 ? err : 0; +} + +/** + * Setting timestamp ODR in batching mode + * + * @param hw: ST IMU MEMS hw instance + * @return Timestamp ODR + */ +static int st_ism330dhcx_ts_odr(struct st_ism330dhcx_hw *hw) +{ + struct st_ism330dhcx_sensor *sensor; + int odr = 0; + u8 i; + + for (i = ST_ISM330DHCX_ID_GYRO; i <= ST_ISM330DHCX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (hw->enable_mask & BIT(sensor->id)) { + odr = max_t(int, odr, sensor->odr); + } + } + + return odr; +} + +/** + * Setting sensor ODR in batching mode + * + * @param sensor: ST IMU sensor instance + * @param enable: enable or disable batching mode + * @return 0 FIFO configured accordingly, non zero otherwise + */ +static inline int +st_ism330dhcx_set_sensor_batching_odr(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + err = __st_ism330dhcx_set_sensor_batching_odr(sensor, enable); + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Update watermark level in FIFO + * + * @param sensor: ST IMU sensor instance + * @param watermark: New watermark level + * @return 0 if FIFO configured, non zero for error + */ +int st_ism330dhcx_update_watermark(struct st_ism330dhcx_sensor *sensor, + u16 watermark) +{ + u16 fifo_watermark = ST_ISM330DHCX_MAX_FIFO_DEPTH, cur_watermark = 0; + struct st_ism330dhcx_hw *hw = sensor->hw; + struct st_ism330dhcx_sensor *cur_sensor; + __le16 wdata; + int i, err; + u8 data; + + for (i = ST_ISM330DHCX_ID_GYRO; i <= ST_ISM330DHCX_ID_STEP_COUNTER; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + + 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->lock); + + err = st_ism330dhcx_read_atomic(hw, ST_ISM330DHCX_REG_FIFO_CTRL1_ADDR + 1, + sizeof(data), &data); + if (err < 0) + goto out; + + fifo_watermark = ((data << 8) & ~ST_ISM330DHCX_REG_FIFO_WTM_MASK) | + (fifo_watermark & ST_ISM330DHCX_REG_FIFO_WTM_MASK); + wdata = cpu_to_le16(fifo_watermark); + err = st_ism330dhcx_write_atomic(hw, ST_ISM330DHCX_REG_FIFO_CTRL1_ADDR, + sizeof(wdata), (u8 *)&wdata); +out: + mutex_unlock(&hw->lock); + + return err < 0 ? err : 0; +} + +/** + * Timestamp correlation finction + * + * @param hw: ST IMU MEMS hw instance + * @param ts: New timestamp + */ +static inline void st_ism330dhcx_sync_hw_ts(struct st_ism330dhcx_hw *hw, s64 ts) +{ + s64 delta = ts - hw->hw_ts; + + hw->ts_offset = st_ism330dhcx_ewma(hw->ts_offset, delta, + ST_ISM330DHCX_EWMA_LEVEL); +} + +/** + * Return the iio device structure based on FIFO TAG ID + * + * @param hw: ST IMU MEMS hw instance + * @param tag: FIFO sample TAG ID + * @return 0 if FIFO configured, non zero for error + */ +static struct +iio_dev *st_ism330dhcx_get_iiodev_from_tag(struct st_ism330dhcx_hw *hw, + u8 tag) +{ + struct iio_dev *iio_dev; + + switch (tag) { + case ST_ISM330DHCX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_GYRO]; + break; + case ST_ISM330DHCX_ACC_TAG: + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_ACC]; + break; + case ST_ISM330DHCX_TEMP_TAG: + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_TEMP]; + break; + case ST_ISM330DHCX_EXT0_TAG: + if (hw->enable_mask & BIT(ST_ISM330DHCX_ID_EXT0)) + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_EXT0]; + else + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_EXT1]; + break; + case ST_ISM330DHCX_EXT1_TAG: + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_EXT1]; + break; + case ST_ISM330DHCX_SC_TAG: + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_STEP_COUNTER]; + break; + default: + iio_dev = NULL; + break; + } + + return iio_dev; +} + +/** + * Read all FIFO data stored after WTM FIFO irq fired interrupt + * + * @param hw: ST IMU MEMS hw instance + * @return Number of read bytes in FIFO or error if negative + */ +static int st_ism330dhcx_read_fifo(struct st_ism330dhcx_hw *hw) +{ + u8 iio_buf[ALIGN(ST_ISM330DHCX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)]; + /* acc + gyro + 2 ext + ts + sc */ + u8 buf[6 * ST_ISM330DHCX_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + struct st_ism330dhcx_sensor *sensor; + 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_ism330dhcx_read_atomic(hw, ST_ISM330DHCX_REG_FIFO_STATUS1_ADDR, + sizeof(fifo_status), (u8 *)&fifo_status); + if (err < 0) + return err; + + fifo_depth = le16_to_cpu(fifo_status) & ST_ISM330DHCX_REG_FIFO_STATUS_DIFF; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_ISM330DHCX_FIFO_SAMPLE_SIZE; + read_len = 0; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_ism330dhcx_read_atomic(hw, + ST_ISM330DHCX_REG_FIFO_DATA_OUT_TAG_ADDR, + word_len, buf); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_ISM330DHCX_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_ISM330DHCX_TAG_SIZE]; + tag = buf[i] >> 3; + + if (tag == ST_ISM330DHCX_TS_TAG) { + val = get_unaligned_le32(ptr); + + if (hw->val_ts_old > val) + hw->hw_ts_high++; + + 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_ism330dhcx_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_ISM330DHCX_EWMA_LEVEL); + + if (!test_bit(ST_ISM330DHCX_HW_FLUSH, &hw->state)) + /* sync ap timestamp and sensor one */ + st_ism330dhcx_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_ism330dhcx_get_iiodev_from_tag(hw, tag); + if (!iio_dev) + continue; + + sensor = iio_priv(iio_dev); + + /* skip samples if not ready */ + drdymask = + (s16)le16_to_cpu(get_unaligned_le16(ptr)); + if (unlikely(drdymask >= + ST_ISM330DHCX_SAMPLE_DISCHARD)) { + continue; + } + + /* + * hw ts in not queued in FIFO if only step + * counter enabled + */ + if (sensor->id == ST_ISM330DHCX_ID_STEP_COUNTER) { + val = get_unaligned_le32(ptr + 2); + hw->tsample = (val + + ((s64)hw->hw_ts_high << 32)) * + hw->ts_delta_ns; + } + + memcpy(iio_buf, ptr, ST_ISM330DHCX_SAMPLE_SIZE); + + /* avoid samples in the future */ + hw->tsample = min_t(s64, + st_ism330dhcx_get_time_ns(hw), + hw->tsample); + + sensor->last_fifo_timestamp = hw->tsample; + + /* support decimation for ODR < 12.5 Hz */ + if (sensor->dec_counter > 0) { + sensor->dec_counter--; + } else { + sensor->dec_counter = sensor->decimator; + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + } + } + } + read_len += word_len; + } + + return read_len; +} + +/** + * Return the max FIFO watermark level accepted + * + * @param dev: Linux Device + * @param attr: Device Attribute + * @param buf: User Buffer + * @return Number of chars printed into the buffer + */ +ssize_t st_ism330dhcx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +/** + * Return the FIFO watermark level + * + * @param dev: Linux Device + * @param attr: Device Attribute + * @param buf: User Buffer + * @return Number of chars printed into the buffer + */ +ssize_t st_ism330dhcx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +/** + * Set the FIFO watermark level + * + * @param dev: Linux Device + * @param attr: Device Attribute + * @param buf: User Buffer + * @param size: New FIFO watermark level + * @return Watermark level if >= 0, error otherwise + */ +ssize_t st_ism330dhcx_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_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int err, val; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto out; + } + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto out; + + err = st_ism330dhcx_update_watermark(sensor, val); + if (err < 0) + goto out; + + sensor->watermark = val; + +out: + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : size; +} + +/** + * Flush internal HW FIFO + * + * @param dev: Linux Device + * @param attr: Device Attribute + * @param buf: User Buffer + * @param size: unused + * @return Watermark level if >= 0, error otherwise + */ +ssize_t st_ism330dhcx_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_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + struct st_ism330dhcx_hw *hw = sensor->hw; + s64 type; + s64 event; + int count; + s64 fts; + s64 ts; + + mutex_lock(&hw->fifo_lock); + ts = st_ism330dhcx_get_time_ns(hw); + hw->delta_ts = ts - hw->ts; + hw->ts = ts; + set_bit(ST_ISM330DHCX_HW_FLUSH, &hw->state); + count = st_ism330dhcx_read_fifo(hw); + sensor->dec_counter = 0; + mutex_unlock(&hw->fifo_lock); + if (count > 0) + fts = sensor->last_fifo_timestamp; + else + fts = ts; + + type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fts); + + return size; +} + +/** + * Empty FIFO and set HW FIFO in Bypass mode + * + * @param hw: ST IMU MEMS hw instance + * @return Watermark level if >= 0, error otherwise + */ +int st_ism330dhcx_suspend_fifo(struct st_ism330dhcx_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + st_ism330dhcx_read_fifo(hw); + err = st_ism330dhcx_set_fifo_mode(hw, ST_ISM330DHCX_FIFO_BYPASS); + mutex_unlock(&hw->fifo_lock); + + return err; +} + +/** + * Update ODR batching in FIFO and Timestamp + * + * @param iio_dev: Linux IIO device + * @param enable: enable/disable batcing in FIFO + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_update_batching(struct iio_dev *iio_dev, bool enable) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + struct st_ism330dhcx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + + err = st_ism330dhcx_set_sensor_batching_odr(sensor, enable); + if (err < 0) + goto out; + + /* Calc TS ODR */ + hw->odr = st_ism330dhcx_ts_odr(hw); + +out: + enable_irq(hw->irq); + + return err; +} + +/** + * Update FIFO watermark value based to the enabled sensors + * + * @param iio_dev: Linux IIO device + * @param enable: enable/disable batcing in FIFO + * @return < 0 if error, 0 otherwise + */ +static int st_ism330dhcx_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + struct st_ism330dhcx_hw *hw = sensor->hw; + int err; + int podr, puodr; + + disable_irq(hw->irq); + + if (sensor->id == ST_ISM330DHCX_ID_EXT0 || + sensor->id == ST_ISM330DHCX_ID_EXT1) { + err = st_ism330dhcx_shub_set_enable(sensor, enable); + if (err < 0) + goto out; + } else { + if (sensor->id == ST_ISM330DHCX_ID_STEP_COUNTER) { + err = st_ism330dhcx_step_counter_set_enable(sensor, + enable); + if (err < 0) + goto out; + } else { + err = st_ism330dhcx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + + err = st_ism330dhcx_set_sensor_batching_odr(sensor, + enable); + if (err < 0) + goto out; + } + } + + /* + * this is an auxiliary sensor, it need to get batched + * toghether at least with a primary sensor (Acc/Gyro) + */ + if (sensor->id == ST_ISM330DHCX_ID_TEMP) { + if (!(hw->enable_mask & (BIT(ST_ISM330DHCX_ID_ACC) | + BIT(ST_ISM330DHCX_ID_GYRO)))) { + struct st_ism330dhcx_sensor *acc_sensor; + u8 data = 0; + + acc_sensor = iio_priv(hw->iio_devs[ST_ISM330DHCX_ID_ACC]); + if (enable) { + err = st_ism330dhcx_get_odr_val(ST_ISM330DHCX_ID_ACC, + sensor->odr, sensor->uodr, + &podr, &puodr, &data); + if (err < 0) + goto out; + } + + err = st_ism330dhcx_write_with_mask(hw, + acc_sensor->batch_reg.addr, + acc_sensor->batch_reg.mask, + data); + if (err < 0) + goto out; + } + } + + err = st_ism330dhcx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + /* Calc TS ODR */ + hw->odr = st_ism330dhcx_ts_odr(hw); + + if (enable && hw->fifo_mode == ST_ISM330DHCX_FIFO_BYPASS) { + st_ism330dhcx_reset_hwts(hw); + err = st_ism330dhcx_set_fifo_mode(hw, ST_ISM330DHCX_FIFO_CONT); + } else if (!hw->enable_mask) { + err = st_ism330dhcx_set_fifo_mode(hw, ST_ISM330DHCX_FIFO_BYPASS); + } + +out: + enable_irq(hw->irq); + + return err; +} + +/** + * Bottom handler for FSM Orientation sensor event generation + * + * @param irq: IIO trigger irq number + * @param p: iio poll function environment + * @return IRQ_HANDLED or < 0 for error + */ +static irqreturn_t st_ism330dhcx_buffer_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + u8 buffer[sizeof(u8) + sizeof(s64)]; + int err; + + err = st_ism330dhcx_fsm_get_orientation(sensor->hw, buffer); + if (err < 0) + goto out; + + iio_push_to_buffers_with_timestamp(iio_dev, buffer, + st_ism330dhcx_get_time_ns(sensor->hw)); +out: + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +/** + * Top handler for sensor event generation + FIFO management + * + * @param irq: IIO trigger irq number + * @param private: iio poll function environment + * @return IRQ_HANDLED or < 0 for error + */ +static irqreturn_t st_ism330dhcx_handler_irq(int irq, void *private) +{ + struct st_ism330dhcx_hw *hw = (struct st_ism330dhcx_hw *)private; + s64 ts = st_ism330dhcx_get_time_ns(hw); + + hw->delta_ts = ts - hw->ts; + hw->ts = ts; + + return IRQ_WAKE_THREAD; +} + +/** + * Bottom handler for sensor event generation + FIFO management + * + * @param irq: irq line number + * @param private: device private environment pointer + * @return IRQ_HANDLED or < 0 for error + */ +static irqreturn_t st_ism330dhcx_handler_thread(int irq, void *private) +{ + struct st_ism330dhcx_hw *hw = (struct st_ism330dhcx_hw *)private; + + mutex_lock(&hw->fifo_lock); + st_ism330dhcx_read_fifo(hw); + clear_bit(ST_ISM330DHCX_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + + if (hw->enable_mask & (BIT(ST_ISM330DHCX_ID_STEP_DETECTOR) | + BIT(ST_ISM330DHCX_ID_SIGN_MOTION) | + BIT(ST_ISM330DHCX_ID_TILT) | + BIT(ST_ISM330DHCX_ID_MOTION) | + BIT(ST_ISM330DHCX_ID_NO_MOTION) | + BIT(ST_ISM330DHCX_ID_WAKEUP) | + BIT(ST_ISM330DHCX_ID_PICKUP) | + BIT(ST_ISM330DHCX_ID_ORIENTATION) | + BIT(ST_ISM330DHCX_ID_WRIST_TILT) | + BIT(ST_ISM330DHCX_ID_GLANCE))) { + struct iio_dev *iio_dev; + u8 status[3]; + s64 event; + int err; + + err = hw->tf->read(hw->dev, + ST_ISM330DHCX_REG_EMB_FUNC_STATUS_MAINPAGE, + sizeof(status), status); + if (err < 0) + return IRQ_HANDLED; + + /* embedded function sensors */ + if (status[0] & ST_ISM330DHCX_REG_INT_STEP_DET_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_STEP_DETECTOR]; + event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[0] & ST_ISM330DHCX_REG_INT_SIGMOT_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_SIGN_MOTION]; + event = IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[0] & ST_ISM330DHCX_REG_INT_TILT_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_TILT]; + event = IIO_UNMOD_EVENT_CODE(IIO_TILT, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + /* fsm sensors */ + if (status[1] & ST_ISM330DHCX_REG_INT_GLANCE_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_GLANCE]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[1] & ST_ISM330DHCX_REG_INT_MOTION_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_MOTION]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[1] & ST_ISM330DHCX_REG_INT_NO_MOTION_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_NO_MOTION]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[1] & ST_ISM330DHCX_REG_INT_WAKEUP_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_WAKEUP]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[1] & ST_ISM330DHCX_REG_INT_PICKUP_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_PICKUP]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + if (status[1] & ST_ISM330DHCX_REG_INT_ORIENTATION_MASK) { + struct st_ism330dhcx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_ORIENTATION]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + if (status[1] & ST_ISM330DHCX_REG_INT_WRIST_MASK) { + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_WRIST_TILT]; + event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + st_ism330dhcx_get_time_ns(hw)); + } + } + + return IRQ_HANDLED; +} + +/** + * IIO fifo pre enabled callback function + * + * @param iio_dev: IIO device + * @return < 0 if error, 0 otherwise + */ +static int st_ism330dhcx_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_ism330dhcx_update_fifo(iio_dev, true); +} + +/** + * IIO fifo post disable callback function + * + * @param iio_dev: IIO device + * @return < 0 if error, 0 otherwise + */ +static int st_ism330dhcx_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_ism330dhcx_update_fifo(iio_dev, false); +} + +/** + * IIO fifo callback registruction structure + */ +static const struct iio_buffer_setup_ops st_ism330dhcx_fifo_ops = { + .preenable = st_ism330dhcx_fifo_preenable, + .postdisable = st_ism330dhcx_fifo_postdisable, +}; + +/** + * Enable HW FIFO + * + * @param hw: ST IMU MEMS hw instance + * @return < 0 if error, 0 otherwise + */ +static int st_ism330dhcx_fifo_init(struct st_ism330dhcx_hw *hw) +{ + return st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_FIFO_CTRL4_ADDR, + ST_ISM330DHCX_REG_DEC_TS_MASK, 1); +} + +static const struct iio_trigger_ops st_ism330dhcx_trigger_ops = { + NULL +}; + +static int st_ism330dhcx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_ism330dhcx_embfunc_sensor_set_enable(iio_priv(iio_dev), true); +} + +static int st_ism330dhcx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_ism330dhcx_embfunc_sensor_set_enable(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_ism330dhcx_buffer_ops = { + .preenable = st_ism330dhcx_buffer_preenable, + .postdisable = st_ism330dhcx_buffer_postdisable, +}; + +/** + * Init IIO buffers and triggers + * + * @param hw: ST IMU MEMS hw instance + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_buffers_setup(struct st_ism330dhcx_hw *hw) +{ + struct device_node *np = hw->dev->of_node; + struct st_ism330dhcx_sensor *sensor; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + struct iio_dev *iio_dev; + 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_ism330dhcx_write_with_mask(hw, ST_ISM330DHCX_REG_CTRL3_C_ADDR, + ST_ISM330DHCX_REG_H_LACTIVE_MASK, + irq_active_low); + if (err < 0) + return err; + + if (np && of_property_read_bool(np, "drive-open-drain")) { + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_CTRL3_C_ADDR, + ST_ISM330DHCX_REG_PP_OD_MASK, 1); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_ism330dhcx_handler_irq, + st_ism330dhcx_handler_thread, + irq_type | IRQF_ONESHOT, + "ism330dhcx", hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return err; + } + + for (i = ST_ISM330DHCX_ID_GYRO; i <= ST_ISM330DHCX_ID_STEP_COUNTER; i++) { + if (!hw->iio_devs[i]) + continue; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + INDIO_BUFFER_SOFTWARE, + &st_ism330dhcx_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_ism330dhcx_fifo_ops; +#endif /* LINUX_VERSION_CODE */ + + } + + err = st_ism330dhcx_fifo_init(hw); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_ISM330DHCX_ID_ORIENTATION]; + sensor = iio_priv(iio_dev); + + err = devm_iio_triggered_buffer_setup(hw->dev, iio_dev, + NULL, st_ism330dhcx_buffer_handler_thread, + &st_ism330dhcx_buffer_ops); + if (err < 0) + return err; + + sensor->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger", + iio_dev->name); + if (!sensor->trig) + return -ENOMEM; + + iio_trigger_set_drvdata(sensor->trig, iio_dev); + sensor->trig->ops = &st_ism330dhcx_trigger_ops; + sensor->trig->dev.parent = hw->dev; + iio_dev->trig = iio_trigger_get(sensor->trig); + + return devm_iio_trigger_register(hw->dev, sensor->trig); +} diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_core.c b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_core.c new file mode 100644 index 000000000000..cabe51a0b7d3 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_core.c @@ -0,0 +1,1908 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_ism330dhcx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_ism330dhcx.h" + +static struct st_ism330dhcx_suspend_resume_entry + st_ism330dhcx_suspend_resume[ST_ISM330DHCX_SUSPEND_RESUME_REGS] = { + [ST_ISM330DHCX_CTRL1_XL_REG] = { + .addr = ST_ISM330DHCX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_ISM330DHCX_CTRL2_G_REG] = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_ISM330DHCX_REG_CTRL3_C_REG] = { + .addr = ST_ISM330DHCX_REG_CTRL3_C_ADDR, + .mask = ST_ISM330DHCX_REG_BDU_MASK | + ST_ISM330DHCX_REG_PP_OD_MASK | + ST_ISM330DHCX_REG_H_LACTIVE_MASK, + }, + [ST_ISM330DHCX_REG_CTRL4_C_REG] = { + .addr = ST_ISM330DHCX_REG_CTRL4_C_ADDR, + .mask = ST_ISM330DHCX_REG_DRDY_MASK, + }, + [ST_ISM330DHCX_REG_CTRL5_C_REG] = { + .addr = ST_ISM330DHCX_REG_CTRL5_C_ADDR, + .mask = ST_ISM330DHCX_REG_ROUNDING_MASK, + }, + [ST_ISM330DHCX_REG_CTRL10_C_REG] = { + .addr = ST_ISM330DHCX_REG_CTRL10_C_ADDR, + .mask = ST_ISM330DHCX_REG_TIMESTAMP_EN_MASK, + }, + [ST_ISM330DHCX_REG_TAP_CFG0_REG] = { + .addr = ST_ISM330DHCX_REG_TAP_CFG0_ADDR, + .mask = ST_ISM330DHCX_REG_LIR_MASK, + }, + [ST_ISM330DHCX_REG_INT1_CTRL_REG] = { + .addr = ST_ISM330DHCX_REG_INT1_CTRL_ADDR, + .mask = ST_ISM330DHCX_REG_INT_FIFO_TH_MASK, + }, + [ST_ISM330DHCX_REG_INT2_CTRL_REG] = { + .addr = ST_ISM330DHCX_REG_INT2_CTRL_ADDR, + .mask = ST_ISM330DHCX_REG_INT_FIFO_TH_MASK, + }, + [ST_ISM330DHCX_REG_FIFO_CTRL1_REG] = { + .addr = ST_ISM330DHCX_REG_FIFO_CTRL1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_ISM330DHCX_REG_FIFO_CTRL2_REG] = { + .addr = ST_ISM330DHCX_REG_FIFO_CTRL2_ADDR, + .mask = ST_ISM330DHCX_REG_FIFO_WTM8_MASK, + }, + [ST_ISM330DHCX_REG_FIFO_CTRL3_REG] = { + .addr = ST_ISM330DHCX_REG_FIFO_CTRL3_ADDR, + .mask = ST_ISM330DHCX_REG_BDR_XL_MASK | + ST_ISM330DHCX_REG_BDR_GY_MASK, + }, + [ST_ISM330DHCX_REG_FIFO_CTRL4_REG] = { + .addr = ST_ISM330DHCX_REG_FIFO_CTRL4_ADDR, + .mask = ST_ISM330DHCX_REG_DEC_TS_MASK | + ST_ISM330DHCX_REG_ODR_T_BATCH_MASK, + }, + }; + +/** + * List of supported ODR + * + * The following table is complete list of supported ODR by Acc, Gyro and Temp + * sensors. ODR value can be also decimal (i.e 12.5 Hz) + */ +static const struct st_ism330dhcx_odr_table_entry st_ism330dhcx_odr_table[] = { + [ST_ISM330DHCX_ID_ACC] = { + .odr_size = 8, + .reg = { + .addr = ST_ISM330DHCX_CTRL1_XL_ADDR, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 0, 0, 0x00 }, + .odr_avl[1] = { 12, 500000, 0x01 }, + .odr_avl[2] = { 26, 0, 0x02 }, + .odr_avl[3] = { 52, 0, 0x03 }, + .odr_avl[4] = { 104, 0, 0x04 }, + .odr_avl[5] = { 208, 0, 0x05 }, + .odr_avl[6] = { 416, 0, 0x06 }, + .odr_avl[7] = { 833, 0, 0x07 }, + }, + [ST_ISM330DHCX_ID_GYRO] = { + .odr_size = 8, + .reg = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 0, 0, 0x00 }, + .odr_avl[1] = { 12, 500000, 0x01 }, + .odr_avl[2] = { 26, 0, 0x02 }, + .odr_avl[3] = { 52, 0, 0x03 }, + .odr_avl[4] = { 104, 0, 0x04 }, + .odr_avl[5] = { 208, 0, 0x05 }, + .odr_avl[6] = { 416, 0, 0x06 }, + .odr_avl[7] = { 833, 0, 0x07 }, + }, + [ST_ISM330DHCX_ID_TEMP] = { + .odr_size = 2, + .odr_avl[0] = { 0, 0, 0x00 }, + .odr_avl[1] = { 12, 500000, 0x02 }, + }, +}; + +/** + * List of supported Full Scale Value + * + * The following table is complete list of supported Full Scale by Acc, Gyro + * and Temp sensors. + */ +static const struct st_ism330dhcx_fs_table_entry st_ism330dhcx_fs_table[] = { + [ST_ISM330DHCX_ID_ACC] = { + .size = ST_ISM330DHCX_FS_ACC_LIST_SIZE, + .fs_avl[0] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ISM330DHCX_ACC_FS_2G_GAIN, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ISM330DHCX_ACC_FS_4G_GAIN, + .val = 0x2, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ISM330DHCX_ACC_FS_8G_GAIN, + .val = 0x3, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = ST_ISM330DHCX_ACC_FS_16G_GAIN, + .val = 0x1, + }, + }, + [ST_ISM330DHCX_ID_GYRO] = { + .size = ST_ISM330DHCX_FS_GYRO_LIST_SIZE, + .fs_avl[0] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ISM330DHCX_GYRO_FS_250_GAIN, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ISM330DHCX_GYRO_FS_500_GAIN, + .val = 0x4, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ISM330DHCX_GYRO_FS_1000_GAIN, + .val = 0x8, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ISM330DHCX_GYRO_FS_2000_GAIN, + .val = 0x0C, + }, + .fs_avl[4] = { + .reg = { + .addr = ST_ISM330DHCX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_ISM330DHCX_GYRO_FS_4000_GAIN, + .val = 0x1, + }, + }, + [ST_ISM330DHCX_ID_TEMP] = { + .size = ST_ISM330DHCX_FS_TEMP_LIST_SIZE, + .fs_avl[0] = { + .reg = { 0 }, + .gain = ST_ISM330DHCX_TEMP_FS_GAIN, + .val = 0x0 + }, + }, +}; + +/** + * Accelerometer IIO channels description + * + * Accelerometer exports to IIO framework the following data channels: + * X Axis (16 bit signed in little endian) + * Y Axis (16 bit signed in little endian) + * Z Axis (16 bit signed in little endian) + * Timestamp (64 bit signed in little endian) + * Accelerometer exports to IIO framework the following event channels: + * Flush event done + */ +static const struct iio_chan_spec st_ism330dhcx_acc_channels[] = { + ST_ISM330DHCX_DATA_CHANNEL(IIO_ACCEL, ST_ISM330DHCX_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's'), + ST_ISM330DHCX_DATA_CHANNEL(IIO_ACCEL, ST_ISM330DHCX_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's'), + ST_ISM330DHCX_DATA_CHANNEL(IIO_ACCEL, ST_ISM330DHCX_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's'), + ST_ISM330DHCX_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +/** + * Gyro IIO channels description + * + * Gyro exports to IIO framework the following data channels: + * X Axis (16 bit signed in little endian) + * Y Axis (16 bit signed in little endian) + * Z Axis (16 bit signed in little endian) + * Timestamp (64 bit signed in little endian) + * Gyro exports to IIO framework the following event channels: + * Flush event done + */ +static const struct iio_chan_spec st_ism330dhcx_gyro_channels[] = { + ST_ISM330DHCX_DATA_CHANNEL(IIO_ANGL_VEL, ST_ISM330DHCX_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's'), + ST_ISM330DHCX_DATA_CHANNEL(IIO_ANGL_VEL, ST_ISM330DHCX_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's'), + ST_ISM330DHCX_DATA_CHANNEL(IIO_ANGL_VEL, ST_ISM330DHCX_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's'), + ST_ISM330DHCX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +/** + * 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_ism330dhcx_step_counter_channels[] = { + { + .type = IIO_STEP_COUNTER, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + ST_ISM330DHCX_EVENT_CHANNEL(IIO_STEP_COUNTER, flush), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +/** + * @brief 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_ism330dhcx_step_detector_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_STEP_DETECTOR, 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_ism330dhcx_sign_motion_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(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_ism330dhcx_tilt_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_TILT, thr), +}; + +/** + * Temperature IIO channels description + * + * Temperature exports to IIO framework the following data channels: + * Temperature (16 bit signed in little endian) + * Temperature exports to IIO framework the following event channels: + * Temperature event threshold + */ +static const struct iio_chan_spec st_ism330dhcx_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_ISM330DHCX_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_ISM330DHCX_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +/** + * Glance IIO channels description + * + * Glance exports to IIO framework the following event channels: + * Glance event detection + */ +static const struct iio_chan_spec st_ism330dhcx_glance_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +/** + * Motion IIO channels description + * + * Motion exports to IIO framework the following event channels: + * Motion event detection + */ +static const struct iio_chan_spec st_ism330dhcx_motion_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +/** + * No Motion IIO channels description + * + * No Motion exports to IIO framework the following event channels: + * No Motion event detection + */ +static const struct iio_chan_spec st_ism330dhcx_no_motion_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +/** + * Wakeup IIO channels description + * + * Wakeup exports to IIO framework the following event channels: + * Wakeup event detection + */ +static const struct iio_chan_spec st_ism330dhcx_wakeup_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +/** + * Pickup IIO channels description + * + * Pickup exports to IIO framework the following event channels: + * Pickup event detection + */ +static const struct iio_chan_spec st_ism330dhcx_pickup_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +/** + * Orientation IIO channels description + * + * Orientation exports to IIO framework the following data channels: + * Orientation (8 bit unsigned in little endian) + * Timestamp (64 bit signed in little endian) + */ +static const struct iio_chan_spec st_ism330dhcx_orientation_channels[] = { + { + .type = IIO_GESTURE, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 8, + .storagebits = 8, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +/** + * Wrist IIO channels description + * + * Wrist exports to IIO framework the following event channels: + * Wrist event detection + */ +static const struct iio_chan_spec st_ism330dhcx_wrist_channels[] = { + ST_ISM330DHCX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +int __st_ism330dhcx_write_with_mask(struct st_ism330dhcx_hw *hw, u8 addr, u8 mask, + u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + goto out; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); + if (err < 0) + dev_err(hw->dev, "failed to write %02x register\n", addr); + +out: + mutex_unlock(&hw->lock); + + return err; +} + +/** + * Detect device ID + * + * Check the value of the Device ID if valid + * + * @param hw: ST IMU MEMS hw instance + * @return 0 if OK, negative value for ERROR + */ +static int st_ism330dhcx_check_whoami(struct st_ism330dhcx_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_ISM330DHCX_REG_WHOAMI_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_ISM330DHCX_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + return 0; +} + +/** + * Get timestamp calibration + * + * Read timestamp calibration data and trim delta time + * + * @param hw: ST IMU MEMS hw instance + * @return 0 if OK, negative value for ERROR + */ +static int st_ism330dhcx_get_odr_calibration(struct st_ism330dhcx_hw *hw) +{ + int err; + s8 data; + s64 odr_calib; + + err = hw->tf->read(hw->dev, ST_ISM330DHCX_INTERNAL_FREQ_FINE, sizeof(data), + (u8 *)&data); + if (err < 0) { + dev_err(hw->dev, "failed to read %d register\n", + ST_ISM330DHCX_INTERNAL_FREQ_FINE); + return err; + } + + odr_calib = (data * 37500) / 1000; + hw->ts_delta_ns = ST_ISM330DHCX_TS_DELTA_NS - odr_calib; + + dev_info(hw->dev, "Freq Fine %lld (ts %lld)\n", + odr_calib, hw->ts_delta_ns); + + return 0; +} + +/** + * Set sensor Full Scale + * + * Set new Full Scale value for a specific sensor + * + * @param sensor: ST IMU sensor instance + * @param gain: New gain value + * @return 0 if OK, negative value for ERROR + */ +static int st_ism330dhcx_set_full_scale(struct st_ism330dhcx_sensor *sensor, u32 gain) +{ + enum st_ism330dhcx_sensor_id id = sensor->id; + int i, err; + u8 val; + + for (i = 0; i < st_ism330dhcx_fs_table[id].size; i++) + if (st_ism330dhcx_fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == st_ism330dhcx_fs_table[id].size) + return -EINVAL; + + val = st_ism330dhcx_fs_table[id].fs_avl[i].val; + err = st_ism330dhcx_write_with_mask(sensor->hw, + st_ism330dhcx_fs_table[id].fs_avl[i].reg.addr, + st_ism330dhcx_fs_table[id].fs_avl[i].reg.mask, + val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +/** + * Get a valid ODR + * + * Check a valid ODR closest to the passed value + * + * @param id: Sensor Identifier + * @param odr: Most significant part of ODR value (in Hz). + * @param uodr: Least significant part of ODR value (in micro Hz). + * @param podr: User data pointer. + * @param puodr: User data pointer. + * @param val: ODR register value data pointer. + * @return 0 if OK, negative value for ERROR + */ +int st_ism330dhcx_get_odr_val(enum st_ism330dhcx_sensor_id id, int odr, int uodr, + int *podr, int *puodr, u8 *val) +{ + int i; + int sensor_odr; + int all_odr = ST_ISM330DHCX_ODR_EXPAND(odr, uodr); + + for (i = 0; i < st_ism330dhcx_odr_table[id].odr_size; i++) { + sensor_odr = + ST_ISM330DHCX_ODR_EXPAND(st_ism330dhcx_odr_table[id].odr_avl[i].hz, + st_ism330dhcx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= all_odr) + break; + } + + if (i == st_ism330dhcx_odr_table[id].odr_size) + return -EINVAL; + + *val = st_ism330dhcx_odr_table[id].odr_avl[i].val; + *podr = st_ism330dhcx_odr_table[id].odr_avl[i].hz; + *puodr = st_ism330dhcx_odr_table[id].odr_avl[i].uhz; + + return 0; +} + +static u16 st_ism330dhcx_check_odr_dependency(struct st_ism330dhcx_hw *hw, + int odr, int uodr, + enum st_ism330dhcx_sensor_id ref_id) +{ + struct st_ism330dhcx_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = odr > 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; +} + +/** + * Set new ODR to sensor + * Set a valid ODR closest to the passed value + * + * @param sensor: ST IMU sensor instance + * @param req_odr: Most significant part of ODR value (in Hz). + * @param req_uodr: Least significant part of ODR value (in micro Hz). + * @return 0 if OK, negative value for ERROR + */ +static int st_ism330dhcx_set_odr(struct st_ism330dhcx_sensor *sensor, int req_odr, + int req_uodr) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + enum st_ism330dhcx_sensor_id id = sensor->id; + int err; + u8 val; + + switch (id) { + case ST_ISM330DHCX_ID_STEP_COUNTER: + case ST_ISM330DHCX_ID_STEP_DETECTOR: + case ST_ISM330DHCX_ID_SIGN_MOTION: + case ST_ISM330DHCX_ID_TILT: + case ST_ISM330DHCX_ID_NO_MOTION: + case ST_ISM330DHCX_ID_MOTION: + case ST_ISM330DHCX_ID_GLANCE: + case ST_ISM330DHCX_ID_WAKEUP: + case ST_ISM330DHCX_ID_PICKUP: + case ST_ISM330DHCX_ID_ORIENTATION: + case ST_ISM330DHCX_ID_WRIST_TILT: + case ST_ISM330DHCX_ID_TEMP: + case ST_ISM330DHCX_ID_EXT0: + case ST_ISM330DHCX_ID_EXT1: + case ST_ISM330DHCX_ID_ACC: { + int odr; + int i; + + id = ST_ISM330DHCX_ID_ACC; + for (i = ST_ISM330DHCX_ID_ACC; i <= ST_ISM330DHCX_ID_TILT; i++) { + if (!hw->iio_devs[i]) + continue; + + if (i == sensor->id) + continue; + + /* req_uodr not used */ + odr = st_ism330dhcx_check_odr_dependency(hw, req_odr, + req_uodr, i); + if (odr != req_odr) + /* device already configured */ + return 0; + } + break; + } + default: + break; + } + + err = st_ism330dhcx_get_odr_val(id, req_odr, req_uodr, &req_odr, + &req_uodr, &val); + if (err < 0) + return err; + + return st_ism330dhcx_write_with_mask(hw, st_ism330dhcx_odr_table[id].reg.addr, + st_ism330dhcx_odr_table[id].reg.mask, + val); +} + +/** + * Enable or Disable sensor + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable or disable the sensor [true,false]. + * @return 0 if OK, negative value for ERROR + */ +int st_ism330dhcx_sensor_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + int uodr = 0; + int odr = 0; + int err; + + if (enable) { + odr = sensor->odr; + uodr = sensor->uodr; + } + + err = st_ism330dhcx_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; +} + +/** + * Single sensor read operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Output data register value. + * @param val: Output data buffer. + * @return IIO_VAL_INT if OK, negative value for ERROR + */ +static int st_ism330dhcx_read_oneshot(struct st_ism330dhcx_sensor *sensor, + u8 addr, int *val) +{ + int err, delay; + __le16 data; + + err = st_ism330dhcx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_ism330dhcx_read_atomic(sensor->hw, addr, sizeof(data), + (u8 *)&data); + if (err < 0) + return err; + + st_ism330dhcx_sensor_set_enable(sensor, false); + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +/** + * Read Sensor data configuration + * + * @param iio_dev: IIO Device. + * @param ch: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_ism330dhcx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + ret = -EBUSY; + mutex_unlock(&iio_dev->mlock); + break; + } + ret = st_ism330dhcx_read_oneshot(sensor, ch->address, val); + mutex_unlock(&iio_dev->mlock); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + 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_ISM330DHCX_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; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * Write Sensor data configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_ism330dhcx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_ism330dhcx_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + int todr, tuodr; + + err = st_ism330dhcx_get_odr_val(sensor->id, val, val2, &todr, + &tuodr, &data); + if (!err) { + sensor->odr = todr; + 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_ISM330DHCX_ID_GYRO: + case ST_ISM330DHCX_ID_ACC: + err = st_ism330dhcx_set_odr(sensor, sensor->odr, + sensor->uodr); + /* I2C interface err can be positive */ + if (err < 0) + break; + + err = st_ism330dhcx_update_batching(iio_dev, 1); + default: + break; + } + } + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +#ifdef CONFIG_DEBUG_FS +static int st_ism330dhcx_reg_access(struct iio_dev *iio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int ret; + + mutex_lock(&iio_dev->mlock); + if (readval == NULL) { + ret = sensor->hw->tf->write(sensor->hw->dev, reg, 1, + (u8 *)&writeval); + } else { + sensor->hw->tf->read(sensor->hw->dev, reg, 1, + (u8 *)readval); + ret = 0; + } + mutex_unlock(&iio_dev->mlock); + + return ret; +} +#endif /* CONFIG_DEBUG_FS */ + +/** + * Read sensor event configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param type: Event Type. + * @param dir: Event Direction. + * @return 1 if Enabled, 0 Disabled + */ +static int st_ism330dhcx_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_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + struct st_ism330dhcx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/** + * Write sensor event configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param type: Event Type. + * @param dir: Event Direction. + * @param state: New event state. + * @return 0 if OK, negative for ERROR + */ +static int st_ism330dhcx_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_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_ism330dhcx_embfunc_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Get a list of available sensor ODR + * + * List of available ODR returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_ism330dhcx_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_ism330dhcx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < ST_ISM330DHCX_ODR_LIST_SIZE; i++) { + if (!st_ism330dhcx_odr_table[id].odr_avl[i].hz) + continue; + + if (st_ism330dhcx_odr_table[id].odr_avl[i].uhz == 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_ism330dhcx_odr_table[id].odr_avl[i].hz); + else + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%d ", + st_ism330dhcx_odr_table[id].odr_avl[i].hz, + st_ism330dhcx_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +/** + * Get a list of available sensor Full Scale + * + * List of available Full Scale returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t st_ism330dhcx_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_ism330dhcx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_ism330dhcx_fs_table[id].size; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_ism330dhcx_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +/** + * Reset step counter value + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @param size: User buffer size. + * @return buffer len, negative for ERROR + */ +static ssize_t +st_ism330dhcx_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 = st_ism330dhcx_reset_step_counter(iio_dev); + + return err < 0 ? err : size; +} + +static int st_ism330dhcx_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == 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 -EINVAL; + } + } + + return -EINVAL; +} + +ssize_t st_ism330dhcx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + struct st_ism330dhcx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_ism330dhcx_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_ism330dhcx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_ism330dhcx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_ism330dhcx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_ism330dhcx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_ism330dhcx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_ism330dhcx_get_watermark, + st_ism330dhcx_set_watermark, 0); +static IIO_DEVICE_ATTR(reset_counter, 0200, NULL, + st_ism330dhcx_sysfs_reset_step_counter, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_ism330dhcx_get_module_id, NULL, 0); + +static struct attribute *st_ism330dhcx_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_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_acc_attribute_group = { + .attrs = st_ism330dhcx_acc_attributes, +}; + +static const struct iio_info st_ism330dhcx_acc_info = { + .attrs = &st_ism330dhcx_acc_attribute_group, + .read_raw = st_ism330dhcx_read_raw, + .write_raw = st_ism330dhcx_write_raw, + .write_raw_get_fmt = st_ism330dhcx_write_raw_get_fmt, +#ifdef CONFIG_DEBUG_FS + /* connect debug info to first device */ + .debugfs_reg_access = st_ism330dhcx_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static struct attribute *st_ism330dhcx_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_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_gyro_attribute_group = { + .attrs = st_ism330dhcx_gyro_attributes, +}; + +static const struct iio_info st_ism330dhcx_gyro_info = { + .attrs = &st_ism330dhcx_gyro_attribute_group, + .read_raw = st_ism330dhcx_read_raw, + .write_raw = st_ism330dhcx_write_raw, + .write_raw_get_fmt = st_ism330dhcx_write_raw_get_fmt, +}; + +static struct attribute *st_ism330dhcx_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_ism330dhcx_temp_attribute_group = { + .attrs = st_ism330dhcx_temp_attributes, +}; + +static const struct iio_info st_ism330dhcx_temp_info = { + .attrs = &st_ism330dhcx_temp_attribute_group, + .read_raw = st_ism330dhcx_read_raw, + .write_raw = st_ism330dhcx_write_raw, + .write_raw_get_fmt = st_ism330dhcx_write_raw_get_fmt, +}; + +static struct attribute *st_ism330dhcx_step_counter_attributes[] = { + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_reset_counter.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_ism330dhcx_step_counter_attribute_group = { + .attrs = st_ism330dhcx_step_counter_attributes, +}; + +static const struct iio_info st_ism330dhcx_step_counter_info = { + .attrs = &st_ism330dhcx_step_counter_attribute_group, +}; + +static struct attribute *st_ism330dhcx_step_detector_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_step_detector_attribute_group = { + .attrs = st_ism330dhcx_step_detector_attributes, +}; + +static const struct iio_info st_ism330dhcx_step_detector_info = { + .attrs = &st_ism330dhcx_step_detector_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_sign_motion_attribute_group = { + .attrs = st_ism330dhcx_sign_motion_attributes, +}; + +static const struct iio_info st_ism330dhcx_sign_motion_info = { + .attrs = &st_ism330dhcx_sign_motion_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_tilt_attribute_group = { + .attrs = st_ism330dhcx_tilt_attributes, +}; + +static const struct iio_info st_ism330dhcx_tilt_info = { + .attrs = &st_ism330dhcx_tilt_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_glance_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_glance_attribute_group = { + .attrs = st_ism330dhcx_glance_attributes, +}; + +static const struct iio_info st_ism330dhcx_glance_info = { + .attrs = &st_ism330dhcx_glance_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_motion_attribute_group = { + .attrs = st_ism330dhcx_motion_attributes, +}; + +static const struct iio_info st_ism330dhcx_motion_info = { + .attrs = &st_ism330dhcx_motion_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_no_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_no_motion_attribute_group = { + .attrs = st_ism330dhcx_no_motion_attributes, +}; + +static const struct iio_info st_ism330dhcx_no_motion_info = { + .attrs = &st_ism330dhcx_no_motion_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_wakeup_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_wakeup_attribute_group = { + .attrs = st_ism330dhcx_wakeup_attributes, +}; + +static const struct iio_info st_ism330dhcx_wakeup_info = { + .attrs = &st_ism330dhcx_wakeup_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_pickup_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_pickup_attribute_group = { + .attrs = st_ism330dhcx_pickup_attributes, +}; + +static const struct iio_info st_ism330dhcx_pickup_info = { + .attrs = &st_ism330dhcx_pickup_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static struct attribute *st_ism330dhcx_orientation_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_orientation_attribute_group = { + .attrs = st_ism330dhcx_orientation_attributes, +}; + +static const struct iio_info st_ism330dhcx_orientation_info = { + .attrs = &st_ism330dhcx_orientation_attribute_group, +}; + +static struct attribute *st_ism330dhcx_wrist_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_ism330dhcx_wrist_attribute_group = { + .attrs = st_ism330dhcx_wrist_attributes, +}; + +static const struct iio_info st_ism330dhcx_wrist_info = { + .attrs = &st_ism330dhcx_wrist_attribute_group, + .read_event_config = st_ism330dhcx_read_event_config, + .write_event_config = st_ism330dhcx_write_event_config, +}; + +static const unsigned long st_ism330dhcx_available_scan_masks[] = { 0x7, 0x0 }; +static const unsigned long st_ism330dhcx_sc_available_scan_masks[] = { 0x1, 0x0 }; + +static int st_ism330dhcx_of_get_pin(struct st_ism330dhcx_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_ism330dhcx_get_int_reg(struct st_ism330dhcx_hw *hw, u8 *drdy_reg, + u8 *ef_irq_reg) +{ + int err = 0, int_pin; + + if (st_ism330dhcx_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: + hw->embfunc_pg0_irq_reg = ST_ISM330DHCX_REG_MD1_CFG_ADDR; + hw->embfunc_irq_reg = ST_ISM330DHCX_REG_EMB_FUNC_INT1_ADDR; + *ef_irq_reg = ST_ISM330DHCX_REG_MD1_CFG_ADDR; + *drdy_reg = ST_ISM330DHCX_REG_INT1_CTRL_ADDR; + break; + case 2: + hw->embfunc_pg0_irq_reg = ST_ISM330DHCX_REG_MD2_CFG_ADDR; + hw->embfunc_irq_reg = ST_ISM330DHCX_REG_EMB_FUNC_INT2_ADDR; + *ef_irq_reg = ST_ISM330DHCX_REG_MD2_CFG_ADDR; + *drdy_reg = ST_ISM330DHCX_REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +static int st_ism330dhcx_reset_device(struct st_ism330dhcx_hw *hw) +{ + int err; + + /* disable I3C */ + err = st_ism330dhcx_write_with_mask(hw, ST_ISM330DHCX_REG_CTRL9_XL_ADDR, + ST_ISM330DHCX_REG_I3C_DISABLE_MASK, 1); + if (err < 0) + return err; + + /* sw reset */ + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_CTRL3_C_ADDR, + ST_ISM330DHCX_REG_SW_RESET_MASK, 1); + if (err < 0) + return err; + + usleep_range(15, 20); + + /* boot */ + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_CTRL3_C_ADDR, + ST_ISM330DHCX_REG_BOOT_MASK, 1); + + msleep(20); + + return err; +} + +static int st_ism330dhcx_init_device(struct st_ism330dhcx_hw *hw) +{ + u8 drdy_reg, ef_irq_reg; + int err; + + /* configure latch interrupts enabled */ + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_TAP_CFG0_ADDR, + ST_ISM330DHCX_REG_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable Block Data Update */ + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_CTRL3_C_ADDR, + ST_ISM330DHCX_REG_BDU_MASK, 1); + if (err < 0) + return err; + + /* enable rouding for fast FIFO reading */ + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_CTRL5_C_ADDR, + ST_ISM330DHCX_REG_ROUNDING_MASK, 3); + if (err < 0) + return err; + + /* init timestamp engine */ + err = st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_CTRL10_C_ADDR, + ST_ISM330DHCX_REG_TIMESTAMP_EN_MASK, 1); + if (err < 0) + return err; + + /* configure interrupt registers */ + err = st_ism330dhcx_get_int_reg(hw, &drdy_reg, &ef_irq_reg); + if (err < 0) + return err; + + /* Enable DRDY MASK for filters settling time */ + err = st_ism330dhcx_write_with_mask(hw, ST_ISM330DHCX_REG_CTRL4_C_ADDR, + ST_ISM330DHCX_REG_DRDY_MASK, 1); + if (err < 0) + return err; + + /* enable FIFO watermak interrupt */ + err = st_ism330dhcx_write_with_mask(hw, drdy_reg, + ST_ISM330DHCX_REG_INT_FIFO_TH_MASK, 1); + if (err < 0) + return err; + + /* enable enbedded function interrupts */ + err = st_ism330dhcx_write_with_mask(hw, ef_irq_reg, + ST_ISM330DHCX_REG_INT_EMB_FUNC_MASK, 1); + if (err < 0) + return err; + + /* init finite state machine */ + return st_ism330dhcx_fsm_init(hw); +} + +/** + * Allocate IIO device + * + * @param hw: ST IMU MEMS hw instance. + * @param id: Sensor Identifier. + * @retval struct iio_dev *, NULL if ERROR + */ +static struct iio_dev *st_ism330dhcx_alloc_iiodev(struct st_ism330dhcx_hw *hw, + enum st_ism330dhcx_sensor_id id) +{ + struct st_ism330dhcx_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; + + switch (id) { + case ST_ISM330DHCX_ID_ACC: + iio_dev->channels = st_ism330dhcx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_acc_channels); + iio_dev->name = "ism330dhcx_accel"; + iio_dev->info = &st_ism330dhcx_acc_info; + iio_dev->available_scan_masks = st_ism330dhcx_available_scan_masks; + + sensor->batch_reg.addr = ST_ISM330DHCX_REG_FIFO_CTRL3_ADDR; + sensor->batch_reg.mask = ST_ISM330DHCX_REG_BDR_XL_MASK; + sensor->max_watermark = ST_ISM330DHCX_MAX_FIFO_DEPTH; + sensor->odr = st_ism330dhcx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_ism330dhcx_odr_table[id].odr_avl[1].uhz; + st_ism330dhcx_set_full_scale(sensor, + st_ism330dhcx_fs_table[id].fs_avl[1].gain); + break; + case ST_ISM330DHCX_ID_GYRO: + iio_dev->channels = st_ism330dhcx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_gyro_channels); + iio_dev->name = "ism330dhcx_gyro"; + iio_dev->info = &st_ism330dhcx_gyro_info; + iio_dev->available_scan_masks = st_ism330dhcx_available_scan_masks; + + sensor->batch_reg.addr = ST_ISM330DHCX_REG_FIFO_CTRL3_ADDR; + sensor->batch_reg.mask = ST_ISM330DHCX_REG_BDR_GY_MASK; + sensor->max_watermark = ST_ISM330DHCX_MAX_FIFO_DEPTH; + sensor->odr = st_ism330dhcx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_ism330dhcx_odr_table[id].odr_avl[1].uhz; + st_ism330dhcx_set_full_scale(sensor, + st_ism330dhcx_fs_table[id].fs_avl[2].gain); + break; + case ST_ISM330DHCX_ID_TEMP: + iio_dev->channels = st_ism330dhcx_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_temp_channels); + iio_dev->name = "ism330dhcx_temp"; + iio_dev->info = &st_ism330dhcx_temp_info; + + sensor->batch_reg.addr = ST_ISM330DHCX_REG_FIFO_CTRL4_ADDR; + sensor->batch_reg.mask = ST_ISM330DHCX_REG_ODR_T_BATCH_MASK; + sensor->max_watermark = ST_ISM330DHCX_MAX_FIFO_DEPTH; + sensor->odr = st_ism330dhcx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_ism330dhcx_odr_table[id].odr_avl[1].uhz; + sensor->gain = st_ism330dhcx_fs_table[id].fs_avl[0].gain; + sensor->offset = ST_ISM330DHCX_TEMP_OFFSET; + break; + case ST_ISM330DHCX_ID_STEP_COUNTER: + iio_dev->channels = st_ism330dhcx_step_counter_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_ism330dhcx_step_counter_channels); + iio_dev->name = "ism330dhcx_step_c"; + iio_dev->info = &st_ism330dhcx_step_counter_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->max_watermark = 1; + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_STEP_DETECTOR: + iio_dev->channels = st_ism330dhcx_step_detector_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_ism330dhcx_step_detector_channels); + iio_dev->name = "ism330dhcx_step_d"; + iio_dev->info = &st_ism330dhcx_step_detector_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_SIGN_MOTION: + iio_dev->channels = st_ism330dhcx_sign_motion_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_ism330dhcx_sign_motion_channels); + iio_dev->name = "ism330dhcx_sign_motion"; + iio_dev->info = &st_ism330dhcx_sign_motion_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_TILT: + iio_dev->channels = st_ism330dhcx_tilt_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_tilt_channels); + iio_dev->name = "ism330dhcx_tilt"; + iio_dev->info = &st_ism330dhcx_tilt_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_GLANCE: + iio_dev->channels = st_ism330dhcx_glance_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_glance_channels); + iio_dev->name = "ism330dhcx_glance"; + iio_dev->info = &st_ism330dhcx_glance_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_MOTION: + iio_dev->channels = st_ism330dhcx_motion_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_motion_channels); + iio_dev->name = "ism330dhcx_motion"; + iio_dev->info = &st_ism330dhcx_motion_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_NO_MOTION: + iio_dev->channels = st_ism330dhcx_no_motion_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_ism330dhcx_no_motion_channels); + iio_dev->name = "ism330dhcx_no_motion"; + iio_dev->info = &st_ism330dhcx_no_motion_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_WAKEUP: + iio_dev->channels = st_ism330dhcx_wakeup_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_wakeup_channels); + iio_dev->name = "ism330dhcx_wk"; + iio_dev->info = &st_ism330dhcx_wakeup_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_PICKUP: + iio_dev->channels = st_ism330dhcx_pickup_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_pickup_channels); + iio_dev->name = "ism330dhcx_pickup"; + iio_dev->info = &st_ism330dhcx_pickup_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_ORIENTATION: + iio_dev->channels = st_ism330dhcx_orientation_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_ism330dhcx_orientation_channels); + iio_dev->name = "ism330dhcx_dev_orientation"; + iio_dev->info = &st_ism330dhcx_orientation_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + case ST_ISM330DHCX_ID_WRIST_TILT: + iio_dev->channels = st_ism330dhcx_wrist_channels; + iio_dev->num_channels = ARRAY_SIZE(st_ism330dhcx_wrist_channels); + iio_dev->name = "ism330dhcx_wrist"; + iio_dev->info = &st_ism330dhcx_wrist_info; + iio_dev->available_scan_masks = + st_ism330dhcx_sc_available_scan_masks; + + sensor->odr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_ism330dhcx_odr_table[ST_ISM330DHCX_ID_ACC].odr_avl[2].uhz; + break; + default: + return NULL; + } + + return iio_dev; +} + +static void st_ism330dhcx_get_properties(struct st_ism330dhcx_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +/** + * Probe device function + * Implements [MODULE] feature for Power Management + * + * @param dev: Device pointer. + * @param irq: I2C/SPI client irq. + * @param tf_ops: Bus Transfer Function pointer. + * @retval struct iio_dev *, NULL if ERROR + */ +int st_ism330dhcx_probe(struct device *dev, int irq, + const struct st_ism330dhcx_transfer_function *tf_ops) +{ + struct st_ism330dhcx_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->tf = tf_ops; + + err = st_ism330dhcx_check_whoami(hw); + if (err < 0) + return err; + + st_ism330dhcx_get_properties(hw); + + err = st_ism330dhcx_get_odr_calibration(hw); + if (err < 0) + return err; + + err = st_ism330dhcx_reset_device(hw); + if (err < 0) + return err; + + err = st_ism330dhcx_init_device(hw); + if (err < 0) + return err; + + for (i = 0; i < ARRAY_SIZE(st_ism330dhcx_main_sensor_list); i++) { + enum st_ism330dhcx_sensor_id id = st_ism330dhcx_main_sensor_list[i]; + + hw->iio_devs[id] = st_ism330dhcx_alloc_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + err = st_ism330dhcx_shub_probe(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_ism330dhcx_buffers_setup(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_ISM330DHCX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + return err; + } + + device_init_wakeup(dev, + device_property_read_bool(dev, "wakeup-source")); + + dev_info(dev, "Device probed\n"); + + return 0; +} +EXPORT_SYMBOL(st_ism330dhcx_probe); + +static int __maybe_unused st_ism330dhcx_bk_regs(struct st_ism330dhcx_hw *hw) +{ + int i, err = 0; + u8 data, addr; + + mutex_lock(&hw->page_lock); + for (i = 0; i < ST_ISM330DHCX_SUSPEND_RESUME_REGS; i++) { + addr = st_ism330dhcx_suspend_resume[i].addr; + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + goto out_lock; + } + + st_ism330dhcx_suspend_resume[i].val = data; + } + +out_lock: + mutex_unlock(&hw->page_lock); + + return err; +} + +static int __maybe_unused st_ism330dhcx_restore_regs(struct st_ism330dhcx_hw *hw) +{ + int i, err = 0; + u8 data, addr; + + mutex_lock(&hw->page_lock); + for (i = 0; i < ST_ISM330DHCX_SUSPEND_RESUME_REGS; i++) { + addr = st_ism330dhcx_suspend_resume[i].addr; + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x reg\n", addr); + goto out_lock; + } + + data &= ~st_ism330dhcx_suspend_resume[i].mask; + data |= (st_ism330dhcx_suspend_resume[i].val & + st_ism330dhcx_suspend_resume[i].mask); + + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to write %02x reg\n", addr); + goto out_lock; + } + } + +out_lock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Power Management suspend callback [MODULE] + * Implements [MODULE] feature for Power Management + * + * @param dev: Device pointer. + * @retval 0 is OK, negative value if ERROR + */ +static int __maybe_unused st_ism330dhcx_suspend(struct device *dev) +{ + struct st_ism330dhcx_hw *hw = dev_get_drvdata(dev); + struct st_ism330dhcx_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Suspending device\n"); + + for (i = 0; i < ST_ISM330DHCX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + /* power off enabled sensors */ + err = st_ism330dhcx_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + + if (st_ism330dhcx_is_fifo_enabled(hw)) { + err = st_ism330dhcx_suspend_fifo(hw); + if (err < 0) + return err; + } + + err = st_ism330dhcx_bk_regs(hw); + + if (device_may_wakeup(dev)) + enable_irq_wake(hw->irq); + + return err < 0 ? err : 0; +} + +/** + * Power Management resume callback [MODULE] + * Implements [MODULE] feature for Power Management + * + * @param dev: Device pointer. + * @retval 0 is OK, negative value if ERROR + */ +static int __maybe_unused st_ism330dhcx_resume(struct device *dev) +{ + struct st_ism330dhcx_hw *hw = dev_get_drvdata(dev); + struct st_ism330dhcx_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Resuming device\n"); + + if (device_may_wakeup(dev)) + disable_irq_wake(hw->irq); + + err = st_ism330dhcx_restore_regs(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_ISM330DHCX_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_ism330dhcx_set_odr(sensor, sensor->odr, sensor->uodr); + if (err < 0) + return err; + } + + err = st_ism330dhcx_reset_hwts(hw); + if (err < 0) + return err; + + if (st_ism330dhcx_is_fifo_enabled(hw)) + err = st_ism330dhcx_set_fifo_mode(hw, ST_ISM330DHCX_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_ism330dhcx_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_ism330dhcx_suspend, st_ism330dhcx_resume) +}; +EXPORT_SYMBOL(st_ism330dhcx_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_ism330dhcx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_embfunc.c b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_embfunc.c new file mode 100644 index 000000000000..a15134b40858 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_embfunc.c @@ -0,0 +1,620 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_ism330dhcx embedded function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2019 STMicroelectronics Inc. + */ + +#include +#include +#include + +#include "st_ism330dhcx.h" + +#define ST_ISM330DHCX_REG_PAGE_SEL_ADDR 0x02 +#define ST_ISM330DHCX_REG_PAGE_SEL_RST_MASK BIT(0) + +#define ST_ISM330DHCX_REG_EMB_FUNC_EN_A_ADDR 0x04 +#define ST_ISM330DHCX_REG_PAGE_ADDRESS 0x08 +#define ST_ISM330DHCX_REG_PAGE_VALUE 0x09 +#define ST_ISM330DHCX_FSM_BASE_ADDRESS 0x2da + +#define ST_ISM330DHCX_REG_PEDO_EN_MASK BIT(3) +#define ST_ISM330DHCX_REG_TILT_EN_MASK BIT(4) +#define ST_ISM330DHCX_REG_SIGN_MOTION_EN_MASK BIT(5) + +#define ST_ISM330DHCX_REG_INT_DTAP_MASK BIT(3) +#define ST_ISM330DHCX_REG_INT_STAP_MASK BIT(6) + +#define ST_ISM330DHCX_REG_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_ISM330DHCX_REG_FSM_EN_MASK BIT(0) +#define ST_ISM330DHCX_REG_INT_STEP_DET_MASK BIT(3) +#define ST_ISM330DHCX_REG_INT_TILT_MASK BIT(4) +#define ST_ISM330DHCX_REG_INT_SIGMOT_MASK BIT(5) + +#define ST_ISM330DHCX_PAGE_RW_ADDR 0x17 +#define ST_ISM330DHCX_REG_WR_MASK GENMASK(6, 5) +#define ST_ISM330DHCX_REG_EMB_FUNC_LIR_MASK BIT(7) + +#define ST_ISM330DHCX_REG_EMB_FUNC_FIFO_CFG_ADDR 0x44 +#define ST_ISM330DHCX_REG_PEDO_FIFO_EN_MASK BIT(6) + +#define ST_ISM330DHCX_REG_FSM_ENABLE_A_ADDR 0x46 +#define ST_ISM330DHCX_REG_FSM_OUTS6_ADDR 0x51 +#define ST_ISM330DHCX_REG_ORIENTATION_0_MASK BIT(5) +#define ST_ISM330DHCX_REG_ORIENTATION_90_MASK BIT(7) +#define ST_ISM330DHCX_REG_ORIENTATION_180_MASK BIT(4) +#define ST_ISM330DHCX_REG_ORIENTATION_270_MASK BIT(6) + +/* Finite State Machine ODR configuration */ +#define ST_ISM330DHCX_REG_EMB_FUNC_ODR_CFG_B_ADDR 0x5f +#define ST_ISM330DHCX_REG_FSM_ODR_MASK GENMASK(5, 3) +#define ST_ISM330DHCX_FSM_ODR_12_5 0 +#define ST_ISM330DHCX_FSM_ODR_26 1 +#define ST_ISM330DHCX_FSM_ODR_52 2 +#define ST_ISM330DHCX_FSM_ODR_104 3 +#define ST_ISM330DHCX_FSM_ODR_208 4 +#define ST_ISM330DHCX_FSM_ODR_416 5 + +#define ST_ISM330DHCX_REG_STEP_COUNTER_L_ADDR 0x62 +#define ST_ISM330DHCX_REG_EMB_FUNC_SRC_ADDR 0x64 +#define ST_ISM330DHCX_REG_PEDO_RST_STEP_MASK BIT(7) + +#define ST_ISM330DHCX_FSM_MAX_SIZE 255 + +/** + * @struct st_ism330dhcx_fsm_sensor + * @brief Single FSM description entry + * + * Implements #595543 Feature + * + * The following FSM state machine ISM330DHCX features listed in EX_FUN_FSM_SENSOR: + * + * SENSOR_TYPE_GLANCE_GESTURE + * SENSOR_TYPE_MOTION_DETECT + * SENSOR_TYPE_STATIONARY_DETECT + * SENSOR_TYPE_WAKE_GESTURE + * SENSOR_TYPE_PICK_UP_GESTURE + * SENSOR_TYPE_WRIST_TILT_GESTURE + * + * will be managed as event sensors + * + * data: FSM binary data block. + * id: Sensor Identifier. + * FSM binary data block len. + */ +struct st_ism330dhcx_fsm_sensor { + u8 data[ST_ISM330DHCX_FSM_MAX_SIZE]; + enum st_ism330dhcx_sensor_id id; + u16 len; +}; + +static const struct st_ism330dhcx_fsm_sensor st_ism330dhcx_fsm_sensor_list[] = { + /* glance */ + { + .id = ST_ISM330DHCX_ID_GLANCE, + .data = { + 0xb2, 0x10, 0x24, 0x20, 0x17, 0x17, 0x66, 0x32, + 0x66, 0x3c, 0x20, 0x20, 0x02, 0x02, 0x08, 0x08, + 0x00, 0x04, 0x0c, 0x00, 0xc7, 0x66, 0x33, 0x73, + 0x77, 0x64, 0x88, 0x75, 0x99, 0x66, 0x33, 0x53, + 0x44, 0xf5, 0x22, 0x00, + }, + .len = 36, + }, + /* motion */ + { + .id = ST_ISM330DHCX_ID_MOTION, + .data = { + 0x51, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3c, + 0x02, 0x00, 0x00, 0x7d, 0x00, 0xc7, 0x05, 0x99, + 0x33, 0x53, 0x44, 0xf5, 0x22, 0x00, + }, + .len = 22, + }, + /* no motion */ + { + .id = ST_ISM330DHCX_ID_NO_MOTION, + .data = { + 0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x66, 0x3c, + 0x02, 0x00, 0x00, 0x7d, 0xff, 0x53, 0x99, 0x50, + }, + .len = 16, + }, + /* wakeup */ + { + .id = ST_ISM330DHCX_ID_WAKEUP, + .data = { + 0xe2, 0x00, 0x1e, 0x20, 0x13, 0x15, 0x66, 0x3e, + 0x66, 0xbe, 0xcd, 0x3c, 0xc0, 0xc0, 0x02, 0x02, + 0x0b, 0x10, 0x05, 0x66, 0xcc, 0x35, 0x38, 0x35, + 0x77, 0xdd, 0x03, 0x54, 0x22, 0x00, + }, + .len = 30, + }, + /* pickup */ + { + .id = ST_ISM330DHCX_ID_PICKUP, + .data = { + 0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x33, 0x3c, + 0x02, 0x00, 0x00, 0x05, 0x05, 0x99, 0x30, 0x00, + }, + .len = 16, + }, + /* orientation */ + { + .id = ST_ISM330DHCX_ID_ORIENTATION, + .data = { + 0x91, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3a, + 0x66, 0x32, 0xf0, 0x00, 0x00, 0x0d, 0x00, 0xc7, + 0x05, 0x73, 0x99, 0x08, 0xf5, 0x22, + }, + .len = 22, + }, + /* wrist tilt */ + { + .id = ST_ISM330DHCX_ID_WRIST_TILT, + .data = { + 0x52, 0x00, 0x14, 0x00, 0x00, 0x00, 0xae, 0xb7, + 0x80, 0x00, 0x00, 0x06, 0x0f, 0x05, 0x73, 0x33, + 0x07, 0x54, 0x44, 0x22, + }, + .len = 20, + }, +}; + +struct st_ism330dhcx_fsm_fs { + u32 gain; + __le16 val; +}; + +static const struct st_ism330dhcx_fsm_fs st_ism330dhcx_fsm_fs_table[] = { + { ST_ISM330DHCX_ACC_FS_2G_GAIN, 0x03ff }, + { ST_ISM330DHCX_ACC_FS_4G_GAIN, 0x07fe }, + { ST_ISM330DHCX_ACC_FS_8G_GAIN, 0x0bfe }, + { ST_ISM330DHCX_ACC_FS_16G_GAIN, 0x0ffe }, +}; + +static inline +int st_ism330dhcx_fsm_set_access(struct st_ism330dhcx_hw *hw, bool enable) +{ + u8 val = enable ? 2 : 0; + + return __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_PAGE_RW_ADDR, + ST_ISM330DHCX_REG_WR_MASK, + val); +} + +static int st_ism330dhcx_fsm_write(struct st_ism330dhcx_hw *hw, u16 base_addr, + int len, const u8 *data) +{ + u8 msb, lsb; + int i, err; + + msb = (((base_addr >> 8) & 0xf) << 4) | 1; + lsb = base_addr & 0xff; + + err = hw->tf->write(hw->dev, + ST_ISM330DHCX_REG_PAGE_ADDRESS, + sizeof(lsb), + &lsb); + if (err < 0) + return err; + + err = hw->tf->write(hw->dev, + ST_ISM330DHCX_REG_PAGE_SEL_ADDR, + sizeof(msb), + &msb); + if (err < 0) + return err; + + for (i = 0; i < len; i++) { + err = hw->tf->write(hw->dev, + ST_ISM330DHCX_REG_PAGE_VALUE, + sizeof(u8), + &data[i]); + if (err < 0) + return err; + + if (++lsb == 0) { + msb += (1 << 4); + err = hw->tf->write(hw->dev, + ST_ISM330DHCX_REG_PAGE_SEL_ADDR, + sizeof(msb), + &msb); + if (err < 0) + return err; + } + } + + return err; +} + +static int st_ism330dhcx_ef_pg1_sensor_set_enable(struct st_ism330dhcx_sensor *sensor, + u8 mask, u8 irq_mask, + bool enable) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + int err; + + err = st_ism330dhcx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, + true); + if (err < 0) + goto unlock; + + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_EMB_FUNC_EN_A_ADDR, + mask, enable); + if (err < 0) + goto reset_page; + + err = __st_ism330dhcx_write_with_mask(hw, hw->embfunc_irq_reg, irq_mask, + enable); +reset_page: + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, false); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * FSM Function sensor [FSM_FUN] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor + * @return < 0 if error, 0 otherwise + */ +static int st_ism330dhcx_fsm_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + u16 enable_mask = hw->fsm_enable_mask; + int err, i; + + for (i = 0; i < ARRAY_SIZE(st_ism330dhcx_fsm_sensor_list); i++) + if (st_ism330dhcx_fsm_sensor_list[i].id == sensor->id) + break; + + if (i == ARRAY_SIZE(st_ism330dhcx_fsm_sensor_list)) + return -EINVAL; + + err = st_ism330dhcx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, + true); + if (err < 0) + goto unlock; + + if (enable) + enable_mask |= BIT(i); + else + enable_mask &= ~BIT(i); + + err = hw->tf->write(hw->dev, ST_ISM330DHCX_REG_FSM_ENABLE_A_ADDR, + sizeof(enable_mask), (u8 *)&enable_mask); + if (err < 0) + goto reset_page; + + hw->fsm_enable_mask = enable_mask; + +reset_page: + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, false); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable Embedded Function sensor [EMB_FUN] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_embfunc_sensor_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + int err; + + switch (sensor->id) { + case ST_ISM330DHCX_ID_STEP_DETECTOR: + err = st_ism330dhcx_ef_pg1_sensor_set_enable(sensor, + ST_ISM330DHCX_REG_PEDO_EN_MASK, + ST_ISM330DHCX_REG_INT_STEP_DET_MASK, + enable); + break; + case ST_ISM330DHCX_ID_SIGN_MOTION: + err = st_ism330dhcx_ef_pg1_sensor_set_enable(sensor, + ST_ISM330DHCX_REG_SIGN_MOTION_EN_MASK, + ST_ISM330DHCX_REG_INT_SIGMOT_MASK, + enable); + break; + case ST_ISM330DHCX_ID_TILT: + err = st_ism330dhcx_ef_pg1_sensor_set_enable(sensor, + ST_ISM330DHCX_REG_TILT_EN_MASK, + ST_ISM330DHCX_REG_TILT_EN_MASK, + enable); + break; + case ST_ISM330DHCX_ID_NO_MOTION: + case ST_ISM330DHCX_ID_MOTION: + case ST_ISM330DHCX_ID_WAKEUP: + case ST_ISM330DHCX_ID_PICKUP: + case ST_ISM330DHCX_ID_ORIENTATION: + case ST_ISM330DHCX_ID_WRIST_TILT: + case ST_ISM330DHCX_ID_GLANCE: + err = st_ism330dhcx_fsm_set_enable(sensor, enable); + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +/** + * Enable Step Counter Sensor [EMB_FUN] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_step_counter_set_enable(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + int err; + + err = st_ism330dhcx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, + true); + if (err < 0) + goto unlock; + + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_EMB_FUNC_EN_A_ADDR, + ST_ISM330DHCX_REG_PEDO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_EMB_FUNC_FIFO_CFG_ADDR, + ST_ISM330DHCX_REG_PEDO_FIFO_EN_MASK, + enable); + +reset_page: + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, false); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Reset Step Counter value [EMB_FUN] + * + * @param iio_dev: IIO device + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_reset_step_counter(struct iio_dev *iio_dev) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + struct st_ism330dhcx_hw *hw = sensor->hw; + u16 prev_val, val = 0; + __le16 data; + int err; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock_iio_dev; + } + + err = st_ism330dhcx_step_counter_set_enable(sensor, true); + if (err < 0) + goto unlock_iio_dev; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, + true); + if (err < 0) + goto unlock_page; + + do { + prev_val = val; + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_EMB_FUNC_SRC_ADDR, + ST_ISM330DHCX_REG_PEDO_RST_STEP_MASK, 1); + if (err < 0) + goto reset_page; + + msleep(100); + + err = hw->tf->read(hw->dev, + ST_ISM330DHCX_REG_STEP_COUNTER_L_ADDR, + sizeof(data), (u8 *)&data); + if (err < 0) + goto reset_page; + + val = le16_to_cpu(data); + } while (val && val >= prev_val); + +reset_page: + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, false); +unlock_page: + mutex_unlock(&hw->page_lock); + + err = st_ism330dhcx_step_counter_set_enable(sensor, false); +unlock_iio_dev: + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Read Orientation data sensor [EMB_FUN] + * + * @param hw: ST IMU MEMS hw instance. + * @param out: Out data buffer. + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_fsm_get_orientation(struct st_ism330dhcx_hw *hw, u8 *out) +{ + int err; + u8 data; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, + true); + if (err < 0) + goto unlock; + + err = hw->tf->read(hw->dev, ST_ISM330DHCX_REG_FSM_OUTS6_ADDR, + sizeof(data), &data); + if (err < 0) + goto reset_page; + + switch (data) { + case ST_ISM330DHCX_REG_ORIENTATION_0_MASK: + *out = 0; + break; + case ST_ISM330DHCX_REG_ORIENTATION_90_MASK: + *out = 1; + break; + case ST_ISM330DHCX_REG_ORIENTATION_180_MASK: + *out = 2; + break; + case ST_ISM330DHCX_REG_ORIENTATION_270_MASK: + *out = 3; + break; + default: + err = -EINVAL; + break; + } + +reset_page: + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, false); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + + +/** + * Initialize Finite State Machine HW block [FSM_FUN] + * + * @param hw: ST IMU MEMS hw instance + * @return < 0 if error, 0 otherwise + */ +int st_ism330dhcx_fsm_init(struct st_ism330dhcx_hw *hw) +{ + u8 nfsm[] = { + ARRAY_SIZE(st_ism330dhcx_fsm_sensor_list), + ARRAY_SIZE(st_ism330dhcx_fsm_sensor_list) + }; + __le16 irq_mask, fsm_addr = ST_ISM330DHCX_FSM_BASE_ADDRESS; + u8 val[2] = {}; + int i, err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, + true); + if (err < 0) + goto unlock; + + /* enable gesture rec */ + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_EMB_FUNC_EN_B_ADDR, + ST_ISM330DHCX_REG_FSM_EN_MASK, + 1); + if (err < 0) + goto reset_page; + + /* gest rec ODR 52Hz */ + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_EMB_FUNC_ODR_CFG_B_ADDR, + ST_ISM330DHCX_REG_FSM_ODR_MASK, + ST_ISM330DHCX_FSM_ODR_52); + if (err < 0) + goto reset_page; + + /* disable all fsm sensors */ + err = hw->tf->write(hw->dev, ST_ISM330DHCX_REG_FSM_ENABLE_A_ADDR, + sizeof(val), val); + if (err < 0) + goto reset_page; + + /* enable fsm interrupt */ + irq_mask = (1 << ARRAY_SIZE(st_ism330dhcx_fsm_sensor_list)) - 1; + err = hw->tf->write(hw->dev, hw->embfunc_irq_reg + 1, + sizeof(irq_mask), + (u8 *)&irq_mask); + if (err < 0) + goto reset_page; + + /* enable latched interrupts */ + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_PAGE_RW_ADDR, + ST_ISM330DHCX_REG_EMB_FUNC_LIR_MASK, + 1); + if (err < 0) + goto reset_page; + + /* enable access */ + err = st_ism330dhcx_fsm_set_access(hw, true); + if (err < 0) + return err; + + /* # of configured fsm */ + err = st_ism330dhcx_fsm_write(hw, 0x17c, sizeof(nfsm), nfsm); + if (err < 0) + goto reset_access; + + err = st_ism330dhcx_fsm_write(hw, 0x17e, sizeof(fsm_addr), (u8 *) + &fsm_addr); + if (err < 0) + goto reset_access; + + /* configure fsm */ + for (i = 0; i < ARRAY_SIZE(st_ism330dhcx_fsm_sensor_list); i++) { + err = st_ism330dhcx_fsm_write(hw, fsm_addr, + st_ism330dhcx_fsm_sensor_list[i].len, + st_ism330dhcx_fsm_sensor_list[i].data); + if (err < 0) + goto reset_access; + + fsm_addr += st_ism330dhcx_fsm_sensor_list[i].len; + } + +reset_access: + st_ism330dhcx_fsm_set_access(hw, false); + + __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_PAGE_SEL_ADDR, + ST_ISM330DHCX_REG_PAGE_SEL_RST_MASK, 1); +reset_page: + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_FUNC_CFG_MASK, false); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_i2c.c b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_i2c.c new file mode 100644 index 000000000000..5224250498b6 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_i2c.c @@ -0,0 +1,97 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_ism330dhcx i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_ism330dhcx.h" + +static int st_ism330dhcx_i2c_read(struct device *dev, u8 addr, + int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_ism330dhcx_i2c_write(struct device *dev, u8 addr, int len, + const u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[8]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_ism330dhcx_transfer_function st_ism330dhcx_transfer_fn = { + .read = st_ism330dhcx_i2c_read, + .write = st_ism330dhcx_i2c_write, +}; + +static int st_ism330dhcx_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_ism330dhcx_probe(&client->dev, client->irq, + &st_ism330dhcx_transfer_fn); +} + +static const struct of_device_id st_ism330dhcx_i2c_of_match[] = { + { + .compatible = "st,ism330dhcx", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_ism330dhcx_i2c_of_match); + +static const struct i2c_device_id st_ism330dhcx_i2c_id_table[] = { + { ST_ISM330DHCX_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_ism330dhcx_i2c_id_table); + +static struct i2c_driver st_ism330dhcx_driver = { + .driver = { + .name = "st_ism330dhcx_i2c", + .pm = &st_ism330dhcx_pm_ops, + .of_match_table = of_match_ptr(st_ism330dhcx_i2c_of_match), + }, + .probe = st_ism330dhcx_i2c_probe, + .id_table = st_ism330dhcx_i2c_id_table, +}; +module_i2c_driver(st_ism330dhcx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_ism330dhcx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_shub.c b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_shub.c new file mode 100644 index 000000000000..a7be520f8f3b --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_shub.c @@ -0,0 +1,1006 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_ism330dhcx sensor hub library driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_ism330dhcx.h" + +#define ST_ISM330DHCX_REG_MASTER_CONFIG_ADDR 0x14 +#define ST_ISM330DHCX_REG_WRITE_ONCE_MASK BIT(6) +#define ST_ISM330DHCX_REG_MASTER_ON_MASK BIT(2) + +#define ST_ISM330DHCX_REG_SLV0_ADDR 0x15 +#define ST_ISM330DHCX_REG_SLV0_CFG 0x17 +#define ST_ISM330DHCX_REG_SLV1_ADDR 0x18 +#define ST_ISM330DHCX_REG_SLV2_ADDR 0x1b +#define ST_ISM330DHCX_REG_SLV3_ADDR 0x1e +#define ST_ISM330DHCX_REG_DATAWRITE_SLV0_ADDR 0x21 +#define ST_ISM330DHCX_REG_BATCH_EXT_SENS_EN_MASK BIT(3) +#define ST_ISM330DHCX_REG_SLAVE_NUMOP_MASK GENMASK(2, 0) + +#define ST_ISM330DHCX_REG_SLV0_OUT_ADDR 0x02 +#define ST_ISM330DHCX_MAX_SLV_NUM 2 + +/** + * @struct st_ism330dhcx_ext_pwr + * @brief External device Power Management description + * reg: Generic sensor register description. + * off_val: Value to write into register to power off external sensor. + * on_val: Value to write into register for power on external sensor. + */ +struct st_ism330dhcx_ext_pwr { + struct st_ism330dhcx_reg reg; + u8 off_val; + u8 on_val; +}; + +/** + * @struct st_ism330dhcx_ext_dev_settings + * @brief External sensor descritor entry + * i2c_addr: External I2C device address (max two). + * wai_addr: Device ID address. + * wai_val: Device ID value. + * odr_table: ODR sensor table. + * fs_table: Full scale table. + * temp_comp_reg: Temperature compensation registers. + * pwr_table: External device Power Management description. + * off_canc_reg: Offset cancellation registers. + * bdu_reg: Block Data Update registers. + * ext_available_scan_masks: IIO device scan mask. + * ext_channels:IIO device channel specifications. + * ext_chan_depth: Max number of IIO device channel specifications. + * data_len: Sensor output data len. + */ +struct st_ism330dhcx_ext_dev_settings { + u8 i2c_addr[2]; + u8 wai_addr; + u8 wai_val; + struct st_ism330dhcx_odr_table_entry odr_table; + struct st_ism330dhcx_fs_table_entry fs_table; + struct st_ism330dhcx_reg temp_comp_reg; + struct st_ism330dhcx_ext_pwr pwr_table; + struct st_ism330dhcx_reg off_canc_reg; + struct st_ism330dhcx_reg bdu_reg; + unsigned long ext_available_scan_masks[2]; + const struct iio_chan_spec ext_channels[5]; + u8 ext_chan_depth; + u8 data_len; +}; + +static const struct st_ism330dhcx_ext_dev_settings st_ism330dhcx_ext_dev_table[] = { + /* LIS2MDL */ + { + .i2c_addr = { 0x1e }, + .wai_addr = 0x4f, + .wai_val = 0x40, + .odr_table = { + .odr_size = 5, + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + /* + * added 5Hz for CTS coverage, reg value is the same + * for 5 and 10 Hz + */ + .odr_avl[0] = { 5, 1, 0x0 }, + .odr_avl[1] = { 10, 0, 0x0 }, + .odr_avl[2] = { 20, 0, 0x1 }, + .odr_avl[3] = { 50, 0, 0x2 }, + .odr_avl[4] = { 100, 0, 0x3 }, + }, + .fs_table = { + .size = 1, + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + }, + .temp_comp_reg = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc_reg = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu_reg = { + .addr = 0x62, + .mask = BIT(4), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_ISM330DHCX_DATA_CHANNEL(IIO_MAGN, 0x68, + 1, IIO_MOD_X, 0, + 16, 16, 's'), + .ext_channels[1] = ST_ISM330DHCX_DATA_CHANNEL(IIO_MAGN, 0x6a, + 1, IIO_MOD_Y, 1, + 16, 16, 's'), + .ext_channels[2] = ST_ISM330DHCX_DATA_CHANNEL(IIO_MAGN, 0x6c, + 1, IIO_MOD_Z, 2, + 16, 16, 's'), + .ext_channels[3] = ST_ISM330DHCX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + /* LPS22HB */ + { + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb1, + .odr_table = { + .odr_size = 4, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1 }, + .odr_avl[1] = { 10, 0, 0x2 }, + .odr_avl[2] = { 25, 0, 0x3 }, + .odr_avl[3] = { 50, 0, 0x4 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_ISM330DHCX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u'), + .ext_channels[1] = ST_ISM330DHCX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, + /* LPS22HH */ + { + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb3, + .odr_table = { + .odr_size = 5, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1 }, + .odr_avl[1] = { 10, 0, 0x2 }, + .odr_avl[2] = { 25, 0, 0x3 }, + .odr_avl[3] = { 50, 0, 0x4 }, + .odr_avl[4] = { 100, 0, 0x6 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_ISM330DHCX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u'), + .ext_channels[1] = ST_ISM330DHCX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, +}; + +/** + * Wait write trigger [SHUB] + * + * In write on external deivce register, each operation is triggered + * by accel/gyro data ready, this means that wait time depends on ODR + * plus i2c time + * NOTE: Be sure to enable Acc or Gyro before this operation + * + * @param hw: ST IMU MEMS hw instance. + */ +static inline void st_ism330dhcx_shub_wait_complete(struct st_ism330dhcx_hw *hw) +{ + struct st_ism330dhcx_sensor *sensor; + u16 odr; + + sensor = iio_priv(hw->iio_devs[ST_ISM330DHCX_ID_ACC]); + /* check if acc is enabled */ + odr = (hw->enable_mask & BIT(ST_ISM330DHCX_ID_ACC)) ? sensor->odr : 13; + msleep((2000U / odr) + 1); +} + +/** + * Read from sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_read_reg(struct st_ism330dhcx_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_SHUB_REG_MASK, + true); + if (err < 0) + goto out; + + err = hw->tf->read(hw->dev, addr, len, data); + + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_SHUB_REG_MASK, false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Write to sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_write_reg(struct st_ism330dhcx_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_SHUB_REG_MASK, + true); + if (err < 0) + goto out; + + err = hw->tf->write(hw->dev, addr, len, data); + + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_SHUB_REG_MASK, false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable sensor hub interface [SHUB] + * + * NOTE: uses page_lock + * + * @param sensor: ST IMU sensor instance + * @param enable: Master Enable/Disable. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_master_enable(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + struct st_ism330dhcx_hw *hw = sensor->hw; + int err; + + /* enable acc sensor as trigger */ + err = st_ism330dhcx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_SHUB_REG_MASK, + true); + if (err < 0) + goto out; + + err = __st_ism330dhcx_write_with_mask(hw, + ST_ISM330DHCX_REG_MASTER_CONFIG_ADDR, + ST_ISM330DHCX_REG_MASTER_ON_MASK, + enable); + + st_ism330dhcx_set_page_access(hw, ST_ISM330DHCX_REG_SHUB_REG_MASK, false); + +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Read sensor data register from shub interface + * + * NOTE: use SLV3 i2c slave for one-shot read operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_read(struct st_ism330dhcx_sensor *sensor, u8 addr, + u8 *data, int len) +{ + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_ism330dhcx_hw *hw = sensor->hw; + u8 out_addr = ST_ISM330DHCX_REG_SLV0_OUT_ADDR + hw->ext_data_len; + u8 config[3]; + int err; + + config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[1] = addr; + config[2] = len & 0x7; + + err = st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_SLV3_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_ism330dhcx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_ism330dhcx_shub_wait_complete(hw); + + err = st_ism330dhcx_shub_read_reg(hw, out_addr, data, len & 0x7); + + st_ism330dhcx_shub_master_enable(sensor, false); + + memset(config, 0, sizeof(config)); + return st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_SLV3_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface + * + * NOTE: use SLV0 i2c slave for write operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_write(struct st_ism330dhcx_sensor *sensor, u8 addr, + u8 *data, int len) +{ + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_ism330dhcx_hw *hw = sensor->hw; + u8 mconfig = ST_ISM330DHCX_REG_WRITE_ONCE_MASK | 3; + u8 config[3] = {}; + int err, i; + + /* AuxSens = 3 + wr once */ + err = st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_MASTER_CONFIG_ADDR, + &mconfig, sizeof(mconfig)); + if (err < 0) + return err; + + config[0] = ext_info->ext_dev_i2c_addr << 1; + for (i = 0; i < len; i++) { + config[1] = addr + i; + + err = st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_ism330dhcx_shub_write_reg(hw, + ST_ISM330DHCX_REG_DATAWRITE_SLV0_ADDR, + &data[i], 1); + if (err < 0) + return err; + + err = st_ism330dhcx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_ism330dhcx_shub_wait_complete(hw); + + st_ism330dhcx_shub_master_enable(sensor, false); + } + + return st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_SLV0_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface using register bitmask + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param mask: Register bitmask. + * @param val: Data buffer. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_write_with_mask(struct st_ism330dhcx_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_ism330dhcx_shub_read(sensor, addr, &data, sizeof(data)); + if (err < 0) + return err; + + data = ((data & ~mask) | (val << __ffs(mask) & mask)); + + return st_ism330dhcx_shub_write(sensor, addr, &data, sizeof(data)); +} + +/** + * Configure external sensor connected on master I2C interface + * + * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor. + * @return 0 if OK, < 0 if ERROR + */ +static int st_ism330dhcx_shub_config_channels(struct st_ism330dhcx_sensor *sensor, + bool enable) +{ + struct st_ism330dhcx_ext_dev_info *ext_info; + struct st_ism330dhcx_hw *hw = sensor->hw; + struct st_ism330dhcx_sensor *cur_sensor; + u8 config[6] = {}, enable_mask; + int i, j = 0; + + enable_mask = enable ? hw->enable_mask | BIT(sensor->id) + : hw->enable_mask & ~BIT(sensor->id); + + for (i = ST_ISM330DHCX_ID_EXT0; i <= ST_ISM330DHCX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT(cur_sensor->id))) + continue; + + ext_info = &cur_sensor->ext_dev_info; + config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[j + 1] = + ext_info->ext_dev_settings->ext_channels[0].address; + config[j + 2] = ST_ISM330DHCX_REG_BATCH_EXT_SENS_EN_MASK | + (ext_info->ext_dev_settings->data_len & + ST_ISM330DHCX_REG_SLAVE_NUMOP_MASK); + j += 3; + } + + return st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_SLV1_ADDR, + config, sizeof(config)); +} + +/** + * Get a valid ODR [SHUB] + * + * Check a valid ODR closest to the passed value + * + * @param sensor: SST IMU sensor instance. + * @param odr: ODR value (in Hz). + * @param val: ODR register value data pointer. + * @return 0 if OK, negative value for ERROR + */ +static int st_ism330dhcx_shub_get_odr_val(struct st_ism330dhcx_sensor *sensor, + u16 odr, u8 *val) +{ + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.odr_size; i++) + if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr) + break; + + if (i == ext_info->ext_dev_settings->odr_table.odr_size) + return -EINVAL; + + *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val; + + /* set decimator for low ODR */ + sensor->decimator = + ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz; + sensor->dec_counter = 0; + + return 0; +} + +/** + * Set new ODR to sensor [SHUB] + * + * Set a valid ODR closest to the passed value + * + * @param sensor: ST IMU sensor instance + * @param odr: ODR value (in Hz). + * @return 0 if OK, negative value for ERROR + */ +static int st_ism330dhcx_shub_set_odr(struct st_ism330dhcx_sensor *sensor, u16 odr) +{ + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_ism330dhcx_hw *hw = sensor->hw; + u8 odr_val; + int err; + + err = st_ism330dhcx_shub_get_odr_val(sensor, odr, &odr_val); + if (err < 0) + return err; + + if (sensor->odr == odr && (hw->enable_mask & BIT(sensor->id))) + return 0; + + return st_ism330dhcx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + odr_val); +} + +/** + * Enable or Disable sensor [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable or disable the sensor [true,false]. + * @return 0 if OK, negative value for ERROR + */ +int st_ism330dhcx_shub_set_enable(struct st_ism330dhcx_sensor *sensor, bool enable) +{ + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err; + + err = st_ism330dhcx_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + if (enable) { + err = st_ism330dhcx_shub_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + } else { + err = st_ism330dhcx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + 0); + if (err < 0) + return err; + } + + if (ext_info->ext_dev_settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? ext_info->ext_dev_settings->pwr_table.on_val + : ext_info->ext_dev_settings->pwr_table.off_val; + err = st_ism330dhcx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->pwr_table.reg.addr, + ext_info->ext_dev_settings->pwr_table.reg.mask, + val); + if (err < 0) + return err; + } + + return st_ism330dhcx_shub_master_enable(sensor, enable); +} + +static inline u32 st_ism330dhcx_get_unaligned_le24(const u8 *p) +{ + return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; +} + +/** + * Single sensor read operation [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param ch: IIO Channel. + * @param val: Output data register value. + * @return IIO_VAL_INT if OK, negative value for ERROR + */ +static int st_ism330dhcx_shub_read_oneshot(struct st_ism330dhcx_sensor *sensor, + struct iio_chan_spec const *ch, + int *val) +{ + int err, delay, len = ch->scan_type.realbits >> 3; + u8 data[4]; + + if (len > ARRAY_SIZE(data)) + return -ENOMEM; + + err = st_ism330dhcx_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_ism330dhcx_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + st_ism330dhcx_shub_set_enable(sensor, false); + + switch (len) { + case 3: + *val = (s32)st_ism330dhcx_get_unaligned_le24(data); + break; + case 2: + *val = (s16)get_unaligned_le16(data); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +/** + * Read Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param ch: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_ism330dhcx_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + ret = -EBUSY; + mutex_unlock(&iio_dev->mlock); + break; + } + ret = st_ism330dhcx_shub_read_oneshot(sensor, ch, val); + mutex_unlock(&iio_dev->mlock); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * Write Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_ism330dhcx_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_ism330dhcx_shub_get_odr_val(sensor, val, &data); + if (!err) + sensor->odr = val; + break; + } + case IIO_CHAN_INFO_SCALE: + err = 0; + break; + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Get a list of available sensor ODR [SHUB] + * + * List of available ODR returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_ism330dhcx_sysfs_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ST_ISM330DHCX_ODR_LIST_SIZE; i++) { + u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +/** + * Get a list of available sensor Full Scale [SHUB] + * + * List of available Full Scale returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t st_ism330dhcx_sysfs_shub_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_ism330dhcx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->fs_table.size; i++) { + u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_ism330dhcx_sysfs_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_ext_scale_available, 0444, + st_ism330dhcx_sysfs_shub_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_ism330dhcx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_ism330dhcx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_ism330dhcx_get_watermark, + st_ism330dhcx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_ism330dhcx_get_module_id, NULL, 0); + +static struct attribute *st_ism330dhcx_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_ext_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_ism330dhcx_ext_attribute_group = { + .attrs = st_ism330dhcx_ext_attributes, +}; + +static const struct iio_info st_ism330dhcx_ext_info = { + .attrs = &st_ism330dhcx_ext_attribute_group, + .read_raw = st_ism330dhcx_shub_read_raw, + .write_raw = st_ism330dhcx_shub_write_raw, +}; + +/** + * Allocate IIO device [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @param ext_settings: xternal sensor descritor entry. + * @param id: Sensor Identifier. + * @param i2c_addr: external I2C address on master bus. + * @return struct iio_dev *, NULL if ERROR + */ +static struct iio_dev *st_ism330dhcx_shub_alloc_iio_dev(struct st_ism330dhcx_hw *hw, + const struct st_ism330dhcx_ext_dev_settings *ext_settings, + enum st_ism330dhcx_sensor_id id, u8 i2c_addr) +{ + struct st_ism330dhcx_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; + iio_dev->available_scan_masks = ext_settings->ext_available_scan_masks; + iio_dev->info = &st_ism330dhcx_ext_info; + iio_dev->channels = ext_settings->ext_channels; + iio_dev->num_channels = ext_settings->ext_chan_depth; + + switch (iio_dev->channels[0].type) { + case IIO_MAGN: + iio_dev->name = "ism330dhcx_magn"; + break; + case IIO_PRESSURE: + iio_dev->name = "ism330dhcx_press"; + break; + default: + iio_dev->name = "ism330dhcx_ext"; + break; + } + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = ext_settings->odr_table.odr_avl[0].hz; + sensor->gain = ext_settings->fs_table.fs_avl[0].gain; + sensor->max_watermark = ST_ISM330DHCX_MAX_FIFO_DEPTH; + sensor->watermark = 1; + sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr; + sensor->ext_dev_info.ext_dev_settings = ext_settings; + sensor->decimator = 0; + sensor->dec_counter = 0; + + return iio_dev; +} + +static int st_ism330dhcx_shub_init_remote_sensor(struct st_ism330dhcx_sensor *sensor) +{ + struct st_ism330dhcx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err = 0; + + if (ext_info->ext_dev_settings->bdu_reg.addr) + err = st_ism330dhcx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->bdu_reg.addr, + ext_info->ext_dev_settings->bdu_reg.mask, 1); + + if (ext_info->ext_dev_settings->temp_comp_reg.addr) + err = st_ism330dhcx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->temp_comp_reg.addr, + ext_info->ext_dev_settings->temp_comp_reg.mask, 1); + + if (ext_info->ext_dev_settings->off_canc_reg.addr) + err = st_ism330dhcx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->off_canc_reg.addr, + ext_info->ext_dev_settings->off_canc_reg.mask, 1); + + return err; +} + +/** + * Probe device function [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @return 0 if OK, negative for ERROR + */ +int st_ism330dhcx_shub_probe(struct st_ism330dhcx_hw *hw) +{ + const struct st_ism330dhcx_ext_dev_settings *settings; + struct st_ism330dhcx_sensor *acc_sensor, *sensor; + u8 config[3], data, num_ext_dev = 0; + enum st_ism330dhcx_sensor_id id; + int err, i = 0, j; + + acc_sensor = iio_priv(hw->iio_devs[ST_ISM330DHCX_ID_ACC]); + while (i < ARRAY_SIZE(st_ism330dhcx_ext_dev_table) && + num_ext_dev < ST_ISM330DHCX_MAX_SLV_NUM) { + settings = &st_ism330dhcx_ext_dev_table[i]; + + for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) { + if (!settings->i2c_addr[j]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[j] << 1) | 1; + config[1] = settings->wai_addr; + config[2] = 1; + + err = st_ism330dhcx_shub_write_reg(hw, + ST_ISM330DHCX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_ism330dhcx_shub_master_enable(acc_sensor, true); + if (err < 0) + return err; + + st_ism330dhcx_shub_wait_complete(hw); + + err = st_ism330dhcx_shub_read_reg(hw, + ST_ISM330DHCX_REG_SLV0_OUT_ADDR, + &data, sizeof(data)); + + st_ism330dhcx_shub_master_enable(acc_sensor, false); + + if (err < 0) + return err; + + if (data != settings->wai_val) + continue; + + id = ST_ISM330DHCX_ID_EXT0 + num_ext_dev; + hw->iio_devs[id] = st_ism330dhcx_shub_alloc_iio_dev(hw, + settings, id, + settings->i2c_addr[j]); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_ism330dhcx_shub_init_remote_sensor(sensor); + if (err < 0) + return err; + + num_ext_dev++; + hw->ext_data_len += settings->data_len; + break; + } + + i++; + } + + if (!num_ext_dev) + return 0; + + memset(config, 0, sizeof(config)); + err = st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + /* AuxSens = 3 + wr once */ + data = ST_ISM330DHCX_REG_WRITE_ONCE_MASK | 3; + return st_ism330dhcx_shub_write_reg(hw, ST_ISM330DHCX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); +} diff --git a/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_spi.c b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_spi.c new file mode 100644 index 000000000000..64f6492df5fb --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dhcx/st_ism330dhcx_spi.c @@ -0,0 +1,107 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_ism330dhcx spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_ism330dhcx.h" + +#define SENSORS_SPI_READ BIT(7) + +static int st_ism330dhcx_spi_read(struct device *dev, u8 addr, int len, + u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_ism330dhcx_hw *hw = spi_get_drvdata(spi); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_ism330dhcx_spi_write(struct device *dev, u8 addr, int len, + const u8 *data) +{ + struct st_ism330dhcx_hw *hw; + struct spi_device *spi; + + if (len >= ST_ISM330DHCX_TX_MAX_LENGTH) + return -ENOMEM; + + spi = to_spi_device(dev); + hw = spi_get_drvdata(spi); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_ism330dhcx_transfer_function st_ism330dhcx_transfer_fn = { + .read = st_ism330dhcx_spi_read, + .write = st_ism330dhcx_spi_write, +}; + +static int st_ism330dhcx_spi_probe(struct spi_device *spi) +{ + return st_ism330dhcx_probe(&spi->dev, spi->irq, + &st_ism330dhcx_transfer_fn); +} + +static const struct of_device_id st_ism330dhcx_spi_of_match[] = { + { + .compatible = "st,ism330dhcx", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_ism330dhcx_spi_of_match); + +static const struct spi_device_id st_ism330dhcx_spi_id_table[] = { + { ST_ISM330DHCX_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_ism330dhcx_spi_id_table); + +static struct spi_driver st_ism330dhcx_driver = { + .driver = { + .name = "st_ism330dhcx_spi", + .pm = &st_ism330dhcx_pm_ops, + .of_match_table = of_match_ptr(st_ism330dhcx_spi_of_match), + }, + .probe = st_ism330dhcx_spi_probe, + .id_table = st_ism330dhcx_spi_id_table, +}; +module_spi_driver(st_ism330dhcx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_ism330dhcx spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dlc/Kconfig b/drivers/iio/stm/imu/st_ism330dlc/Kconfig new file mode 100644 index 000000000000..9ec228ef96aa --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/Kconfig @@ -0,0 +1,88 @@ +# SPDX-License-Identifier: GPL-2.0-only + +# +# st-ism330dlc drivers for STMicroelectronics combo sensor +# + +menuconfig ST_ISM330DLC_IIO + tristate "STMicroelectronics ISM330DLC sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select ST_ISM330DLC_I2C_IIO if (I2C) + select ST_ISM330DLC_SPI_IIO if (SPI) + help + This driver supports ISM330DLC sensors. + It is a gyroscope/accelerometer combo device. + This driver can be built as a module. The module will be called + st_ism330dlc. + +if ST_ISM330DLC_IIO + +config ST_ISM330DLC_I2C_IIO + tristate + depends on ST_ISM330DLC_IIO + depends on I2C + +config ST_ISM330DLC_SPI_IIO + tristate + depends on ST_ISM330DLC_IIO + depends on SPI + +config ST_ISM330DLC_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on ST_ISM330DLC_IIO + range 0 4096 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +menuconfig ST_ISM330DLC_IIO_MASTER_SUPPORT + bool "I2C master controller" + depends on I2C && ST_ISM330DLC_IIO + default n + help + Added support for I2C master controller. Only one slave sensor is + supported. + +if ST_ISM330DLC_IIO_MASTER_SUPPORT + +config ST_ISM330DLC_ENABLE_INTERNAL_PULLUP + bool "Enabled internals pull-up resistors" + default y + +choice + prompt "External sensor 0" + default ST_ISM330DLC_IIO_EXT0_LIS3MDL + help + Choose the external sensor 0 connected to I2C master. + +config ST_ISM330DLC_IIO_EXT0_LIS3MDL + bool "LIS3MDL" +config ST_ISM330DLC_IIO_EXT0_AKM09911 + bool "AKM09911" +config ST_ISM330DLC_IIO_EXT0_AKM09912 + bool "AKM09912" +config ST_ISM330DLC_IIO_EXT0_AKM09916 + bool "AKM09916" +config ST_ISM330DLC_IIO_EXT0_LPS22HB + bool "LPS22HB" +config ST_ISM330DLC_IIO_EXT0_LIS2MDL + bool "LIS2MDL" +endchoice + +endif + +config ST_ISM330DLC_XL_DATA_INJECTION + bool "Enable XL data injection support" + depends on ST_ISM330DLC_IIO + default n + help + This option enables the accelerometer data injection + support. The device functions may so use an injected + pattern instead of taking the real sensor data. + +endif #ST_ISM330DLC_IIO diff --git a/drivers/iio/stm/imu/st_ism330dlc/Makefile b/drivers/iio/stm/imu/st_ism330dlc/Makefile new file mode 100644 index 000000000000..8fc2f3a30caf --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/Makefile @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Makefile for STMicroelectronics ISM330DLC sensor. +# + +obj-$(CONFIG_ST_ISM330DLC_IIO) += st_ism330dlc.o +st_ism330dlc-objs := st_ism330dlc_core.o +obj-$(CONFIG_ST_ISM330DLC_I2C_IIO) += st_ism330dlc_i2c.o +obj-$(CONFIG_ST_ISM330DLC_SPI_IIO) += st_ism330dlc_spi.o + +st_ism330dlc-$(CONFIG_IIO_BUFFER) += st_ism330dlc_buffer.o +st_ism330dlc-$(CONFIG_IIO_TRIGGER) += st_ism330dlc_trigger.o +st_ism330dlc-$(CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT) += st_ism330dlc_i2c_master.o diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc.h b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc.h new file mode 100644 index 000000000000..f6ea41999b94 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc.h @@ -0,0 +1,359 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics ism330dlc driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_ISM330DLC_H +#define ST_ISM330DLC_H + +#include +#include + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT +#include +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + +#define ISM330DLC_DEV_NAME "ism330dlc" + +enum st_mask_id { + ST_MASK_ID_ACCEL = 0, + ST_MASK_ID_GYRO, + ST_MASK_ID_TILT, + ST_MASK_ID_EXT0, + ST_MASK_ID_SENSOR_HUB, + ST_MASK_ID_DIGITAL_FUNC, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP, +}; + +#define ST_INDIO_DEV_NUM 3 + +#define ST_ISM330DLC_TX_MAX_LENGTH 12 +#define ST_ISM330DLC_RX_MAX_LENGTH 4097 + +#define ST_ISM330DLC_BYTE_FOR_CHANNEL 2 +#define ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE 6 + +#define ST_ISM330DLC_MAX_FIFO_SIZE 4096 +#define ST_ISM330DLC_MAX_FIFO_THRESHOLD 546 +#define ST_ISM330DLC_MAX_FIFO_LENGHT (ST_ISM330DLC_MAX_FIFO_SIZE / \ + ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE) + +#define ST_ISM330DLC_SELFTEST_NA_MS "na" +#define ST_ISM330DLC_SELFTEST_FAIL_MS "fail" +#define ST_ISM330DLC_SELFTEST_PASS_MS "pass" + +#define ST_ISM330DLC_WAKE_UP_SENSORS BIT(ST_MASK_ID_TILT) + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT +#define ST_ISM330DLC_NUM_CLIENTS 1 +#else /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ +#define ST_ISM330DLC_NUM_CLIENTS 0 +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + +#define ST_ISM330DLC_LSM_CHANNELS(device_type, modif, index, mod, \ + endian, sbits, rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +extern const struct iio_event_spec ism330dlc_fifo_flush_event; + +#define ST_ISM330DLC_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &ism330dlc_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_ISM330DLC_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + st_ism330dlc_sysfs_get_hwfifo_enabled,\ + st_ism330dlc_sysfs_set_hwfifo_enabled, 0); + +#define ST_ISM330DLC_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + st_ism330dlc_sysfs_get_hwfifo_watermark,\ + st_ism330dlc_sysfs_set_hwfifo_watermark, 0); + +#define ST_ISM330DLC_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + st_ism330dlc_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_ISM330DLC_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + st_ism330dlc_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_ISM330DLC_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + st_ism330dlc_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +struct st_ism330dlc_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[ST_ISM330DLC_RX_MAX_LENGTH]; + u8 tx_buf[ST_ISM330DLC_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct ism330dlc_out_decimation { + short decimator; + short num_samples; +}; + +struct ism330dlc_fifo_output { + u8 sip; + int64_t deltatime; + int64_t deltatime_default; + int64_t timestamp; + int64_t timestamp_p; + short decimator; + short num_samples; + bool initialized; +}; + +/* struct ism330dlc_data - common data for i2c or spi driver instance + * @name: pointer to the device name (i2c name or spi modalias). + * @enable_digfunc_mask: mask used to enable/disable hw digital functions. + * @enable_sensorhub_mask: mask used to enable/disable sensor-hub feature. + * @irq_enable_fifo_mask: mask used to enable/disable fifo irq. + * @irq_enable_accel_ext_mask: mask used to enable/disable accel irq. + * @hw_odr: physical sensor odr expressed in Hz. + * @v_odr: requested sensor odr by userspace expressed in Hz. + * @hwfifo_enabled: is hwfifo enabled? + * @hwfifo_decimator: hwfifo decimator factor. + * @hwfifo_watermark: hwfifo watermark value. + * @samples_to_discard: samples to discard due to ODR switch. + * @nofifo_decimation: output status when fifo is disabled. + * @fifo_output: output status when fifo is enabled. + * @sensors_enabled: sensors enabled mask. + * @sensors_use_fifo: sensors use fifo mask. + * @accel_odr_dependency: odr dependency: accel, sensor-hub, dig-func. + * @accel_on: accel is going to be enabled during fifo odr switch? + * @magn_on: magn is going to be enabled during fifo odr switch? + * @odr_lock: mutex to avoid race condition during odr switch. + * @fifo_data: fifo data. + * @gyro_selftest_status: gyroscope selftest result. + * @accel_selftest_status: accelerometer selftest result. + * @irq: irq number. + * @timestamp: timestamp value from boot process. + * @module_id: identify iio devices of the same sensor module. + */ +struct ism330dlc_data { + const char *name; + + u16 enable_digfunc_mask; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + u16 enable_sensorhub_mask; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + u16 irq_enable_fifo_mask; + u16 irq_enable_accel_ext_mask; + + unsigned int hw_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int trigger_odr; + + bool hwfifo_enabled[ST_INDIO_DEV_NUM + 1]; + u8 hwfifo_decimator[ST_INDIO_DEV_NUM + 1]; + u16 hwfifo_watermark[ST_INDIO_DEV_NUM + 1]; + u16 fifo_watermark; + + u8 samples_to_discard[ST_INDIO_DEV_NUM + 1]; + u8 samples_to_discard_2[ST_INDIO_DEV_NUM + 1]; + struct ism330dlc_out_decimation nofifo_decimation[ST_INDIO_DEV_NUM + 1]; + struct ism330dlc_fifo_output fifo_output[ST_INDIO_DEV_NUM + 1]; + + u16 sensors_enabled; + u16 sensors_use_fifo; + int accel_odr_dependency[3]; + + bool accel_on; + bool magn_on; + enum fifo_mode fifo_status; + + struct mutex odr_lock; + + u8 *fifo_data; + u8 accel_last_push[6]; + u8 gyro_last_push[6]; + u8 ext0_last_push[6]; + int8_t gyro_selftest_status; + int8_t accel_selftest_status; + + u8 drdy_reg; + int irq; + + s64 timestamp; + int64_t fifo_enable_timestamp; + int64_t slower_counter; + uint8_t slower_id; + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + bool injection_mode; + s64 last_injection_timestamp; + u8 injection_odr; +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + + struct work_struct data_work; + + struct device *dev; + struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + 1]; + struct iio_trigger *trig[ST_INDIO_DEV_NUM + 1]; + struct mutex bank_registers_lock; + struct mutex fifo_lock; + u32 module_id; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + bool ext0_available; + int8_t ext0_selftest_status; + struct mutex i2c_transfer_lock; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + const struct st_ism330dlc_transfer_function *tf; + struct st_ism330dlc_transfer_buffer tb; +}; + +struct st_ism330dlc_transfer_function { + int (*write)(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); + int (*read)(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); +}; + +struct ism330dlc_sensor_data { + struct ism330dlc_data *cdata; + + unsigned int c_gain[3]; + + u8 num_data_channels; + u8 sindex; + u8 data_out_reg; +}; + +int st_ism330dlc_write_data_with_mask(struct ism330dlc_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock); + +int st_ism330dlc_push_data_with_timestamp(struct ism330dlc_data *cdata, + u8 index, u8 *data, + int64_t timestamp); + +int st_ism330dlc_common_probe(struct ism330dlc_data *cdata, int irq); +void st_ism330dlc_common_remove(struct ism330dlc_data *cdata, int irq); + +int st_ism330dlc_set_enable(struct ism330dlc_sensor_data *sdata, + bool enable, bool buffer); +int st_ism330dlc_set_fifo_mode(struct ism330dlc_data *cdata, + enum fifo_mode fm); +int st_ism330dlc_enable_sensor_hub(struct ism330dlc_data *cdata, bool enable, + enum st_mask_id id); +int ism330dlc_read_output_data(struct ism330dlc_data *cdata, int sindex, + bool push); +int st_ism330dlc_set_drdy_irq(struct ism330dlc_sensor_data *sdata, bool state); + +ssize_t st_ism330dlc_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_ism330dlc_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_ism330dlc_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_ism330dlc_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_ism330dlc_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_ism330dlc_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_ism330dlc_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_ism330dlc_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +#ifdef CONFIG_IIO_BUFFER +int st_ism330dlc_allocate_rings(struct ism330dlc_data *cdata); +void st_ism330dlc_deallocate_rings(struct ism330dlc_data *cdata); +int st_ism330dlc_trig_set_state(struct iio_trigger *trig, bool state); +int st_ism330dlc_read_fifo(struct ism330dlc_data *cdata, bool async); +#define ST_ISM330DLC_TRIGGER_SET_STATE (&st_ism330dlc_trig_set_state) +#else /* CONFIG_IIO_BUFFER */ +static inline int st_ism330dlc_allocate_rings(struct ism330dlc_data *cdata) +{ + return 0; +} +static inline void st_ism330dlc_deallocate_rings(struct ism330dlc_data *cdata) +{ +} +static inline int st_ism330dlc_read_fifo(struct ism330dlc_data *cdata, + bool async) +{ + return 0; +} +#define ST_ISM330DLC_TRIGGER_SET_STATE NULL +#endif /* CONFIG_IIO_BUFFER */ + +#ifdef CONFIG_IIO_TRIGGER +int st_ism330dlc_allocate_triggers(struct ism330dlc_data *cdata, + const struct iio_trigger_ops *trigger_ops); +void st_ism330dlc_deallocate_triggers(struct ism330dlc_data *cdata); +void st_ism330dlc_flush_works(void); +#else /* CONFIG_IIO_TRIGGER */ +static inline int st_ism330dlc_allocate_triggers(struct ism330dlc_data *cdata, + const struct iio_trigger_ops *trigger_ops, int irq) +{ + return 0; +} +static inline void st_ism330dlc_deallocate_triggers(struct ism330dlc_data *cdata, + int irq) +{ + return; +} +static inline void st_ism330dlc_flush_works(void) +{ + return; +} +#endif /* CONFIG_IIO_TRIGGER */ + +#ifdef CONFIG_PM +int st_ism330dlc_common_suspend(struct ism330dlc_data *cdata); +int st_ism330dlc_common_resume(struct ism330dlc_data *cdata); +#endif /* CONFIG_PM */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT +int st_ism330dlc_write_embedded_registers(struct ism330dlc_data *cdata, + u8 reg_addr, u8 *data, int len); +int st_ism330dlc_i2c_master_probe(struct ism330dlc_data *cdata); +int st_ism330dlc_i2c_master_exit(struct ism330dlc_data *cdata); +#else /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ +static inline int st_ism330dlc_i2c_master_probe(struct ism330dlc_data *cdata) +{ + return 0; +} +static inline int st_ism330dlc_i2c_master_exit(struct ism330dlc_data *cdata) +{ + return 0; +} +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + +#endif /* ST_ISM330DLC_H */ diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_buffer.c b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_buffer.c new file mode 100644 index 000000000000..d1300089e9d5 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_buffer.c @@ -0,0 +1,552 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism330dlc buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_ism330dlc.h" + +#define ST_ISM330DLC_FIFO_DIFF_L 0x3a +#define ST_ISM330DLC_FIFO_DIFF_MASK 0x07 +#define ST_ISM330DLC_FIFO_DATA_OUT_L 0x3e +#define ST_ISM330DLC_FIFO_DATA_OVR 0x40 +#define ST_ISM330DLC_FIFO_DATA_EMPTY 0x10 + +#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \ + (d) : (c)) : ((b == 0) ? \ + (c) : (d))) + +int st_ism330dlc_push_data_with_timestamp(struct ism330dlc_data *cdata, + u8 index, u8 *data, int64_t timestamp) +{ + int i, n = 0; + struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels; + uint16_t bfch, bfchs_out = 0, bfchs_in = 0; + struct ism330dlc_sensor_data *sdata = iio_priv(cdata->indio_dev[index]); + u8 buff[ALIGN(ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)]; + + if (timestamp <= cdata->fifo_output[index].timestamp_p) + return -EINVAL; + + for (i = 0; i < sdata->num_data_channels; i++) { + bfch = chs[i].scan_type.storagebits >> 3; + + if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) { + memcpy(&buff[bfchs_out], &data[bfchs_in], bfch); + n++; + bfchs_out += bfch; + } + + bfchs_in += bfch; + } + + iio_push_to_buffers_with_timestamp(cdata->indio_dev[index], + buff, timestamp); + + cdata->fifo_output[index].timestamp_p = timestamp; + + return 0; +} + +static void st_ism330dlc_parse_fifo_data(struct ism330dlc_data *cdata, + u16 read_len, int64_t time_top, u16 num_pattern) +{ + int err; + u16 fifo_offset = 0; + u8 gyro_sip, accel_sip; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + u8 ext0_sip; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + while (fifo_offset < read_len) { + gyro_sip = cdata->fifo_output[ST_MASK_ID_GYRO].sip; + accel_sip = cdata->fifo_output[ST_MASK_ID_ACCEL].sip; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + do { + if (gyro_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_GYRO) + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_GYRO].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp += cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp -= cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + cdata->samples_to_discard[ST_MASK_ID_GYRO] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_GYRO] > 0) + cdata->samples_to_discard[ST_MASK_ID_GYRO]--; + else { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].num_samples >= cdata->fifo_output[ST_MASK_ID_GYRO].decimator) { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_GYRO)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_GYRO] == 0) { + err = st_ism330dlc_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_GYRO].initialized = true; + + memcpy(cdata->gyro_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_GYRO]--; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].initialized) { + err = st_ism330dlc_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + cdata->gyro_last_push, + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + } + } + } + } + } + + fifo_offset += ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE; + gyro_sip--; + } + + if (accel_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_ACCEL) + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp += cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp -= cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + cdata->samples_to_discard[ST_MASK_ID_ACCEL] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_ACCEL] > 0) + cdata->samples_to_discard[ST_MASK_ID_ACCEL]--; + else { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples >= cdata->fifo_output[ST_MASK_ID_ACCEL].decimator) { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] == 0) { + err = st_ism330dlc_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].initialized = true; + + memcpy(cdata->accel_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_ACCEL]--; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].initialized) { + err = st_ism330dlc_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + cdata->accel_last_push, + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + } + } + } + } + } + + fifo_offset += ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE; + accel_sip--; + } + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if (ext0_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_EXT0) + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_EXT0].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp += cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp -= cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + cdata->samples_to_discard[ST_MASK_ID_EXT0] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_EXT0] > 0) + cdata->samples_to_discard[ST_MASK_ID_EXT0]--; + else { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].num_samples >= cdata->fifo_output[ST_MASK_ID_EXT0].decimator) { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_EXT0)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_EXT0] == 0) { + err = st_ism330dlc_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_EXT0].initialized = true; + + memcpy(cdata->ext0_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_EXT0]--; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].initialized) { + err = st_ism330dlc_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + cdata->ext0_last_push, + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + } + } + } + } + } + + fifo_offset += ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE; + ext0_sip--; + } + + } while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0)); +#else /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } while ((accel_sip > 0) || (gyro_sip > 0)); +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } +} + +int st_ism330dlc_read_fifo(struct ism330dlc_data *cdata, bool async) +{ + int err; + u8 fifo_status[2]; +#if (CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read; +#endif /* CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO */ + u16 read_len = 0, byte_in_pattern, num_pattern; + int64_t temp_counter = 0, timestamp_diff, slower_deltatime; + + err = cdata->tf->read(cdata, ST_ISM330DLC_FIFO_DIFF_L, + 2, fifo_status, true); + if (err < 0) + return err; + + timestamp_diff = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if (fifo_status[1] & ST_ISM330DLC_FIFO_DATA_OVR) { + st_ism330dlc_set_fifo_mode(cdata, BYPASS); + st_ism330dlc_set_fifo_mode(cdata, CONTINUOS); + dev_err(cdata->dev, "data fifo overrun, failed to read it.\n"); + return -EINVAL; + } + + if (fifo_status[1] & ST_ISM330DLC_FIFO_DATA_EMPTY) + return 0; + + read_len = ((fifo_status[1] & ST_ISM330DLC_FIFO_DIFF_MASK) << 8) | fifo_status[0]; + read_len *= ST_ISM330DLC_BYTE_FOR_CHANNEL; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip + + cdata->fifo_output[ST_MASK_ID_EXT0].sip) * + ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE; +#else /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip) * + ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + if (byte_in_pattern == 0) + return 0; + + num_pattern = read_len / byte_in_pattern; + + read_len = (read_len / byte_in_pattern) * byte_in_pattern; + if (read_len == 0) + return 0; + +#if (CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO == 0) + err = cdata->tf->read(cdata, ST_ISM330DLC_FIFO_DATA_OUT_L, + read_len, cdata->fifo_data, true); + if (err < 0) + return err; +#else /* CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + err = cdata->tf->read(cdata, ST_ISM330DLC_FIFO_DATA_OUT_L, + data_to_read, + &cdata->fifo_data[read_len - data_remaining], + true); + if (err < 0) + return err; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO */ + + cdata->slower_id = MIN_ID(cdata->fifo_output[ST_MASK_ID_GYRO].sip, + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, + ST_MASK_ID_GYRO, ST_MASK_ID_ACCEL); +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + cdata->slower_id = MIN_ID(cdata->fifo_output[cdata->slower_id].sip, + cdata->fifo_output[ST_MASK_ID_EXT0].sip, + cdata->slower_id, ST_MASK_ID_EXT0); +#endif /* CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO */ + + temp_counter = cdata->slower_counter; + cdata->slower_counter += (read_len / byte_in_pattern) * cdata->fifo_output[cdata->slower_id].sip; + + if (async) + goto parse_fifo; + + if (temp_counter > 0) { + slower_deltatime = div64_s64(timestamp_diff - cdata->fifo_enable_timestamp, cdata->slower_counter); + + switch (cdata->slower_id) { + case ST_MASK_ID_ACCEL: + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_GYRO: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_EXT0: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = slower_deltatime; + break; + + default: + break; + } + } else { + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default; + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default; +#endif /* CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO */ + } + +parse_fifo: + st_ism330dlc_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern); + + return 0; +} + +int ism330dlc_read_output_data(struct ism330dlc_data *cdata, int sindex, bool push) +{ + int err; + u8 data[6]; + struct iio_dev *indio_dev = cdata->indio_dev[sindex]; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + err = cdata->tf->read(cdata, sdata->data_out_reg, + ST_ISM330DLC_BYTE_FOR_CHANNEL * 3, data, true); + if (err < 0) + return err; + + if (push) + st_ism330dlc_push_data_with_timestamp(cdata, sindex, + data, cdata->timestamp); + + return 0; +} +EXPORT_SYMBOL(ism330dlc_read_output_data); + +static irqreturn_t st_ism330dlc_outdata_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static inline irqreturn_t st_ism330dlc_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int st_ism330dlc_trig_set_state(struct iio_trigger *trig, bool state) +{ + return 0; +} + +static int st_ism330dlc_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) { + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + case ST_MASK_ID_GYRO: + return -EBUSY; + + default: + break; + } + } +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + + return 0; +} + +static int st_ism330dlc_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[sdata->sindex]) && + (indio_dev->buffer->length < 2 * ST_ISM330DLC_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_ism330dlc_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + return 0; +} + +static int st_ism330dlc_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_ism330dlc_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : 0; +} + +static const struct iio_buffer_setup_ops st_ism330dlc_buffer_setup_ops = { + .preenable = &st_ism330dlc_buffer_preenable, + .postenable = &st_ism330dlc_buffer_postenable, + .postdisable = &st_ism330dlc_buffer_postdisable, +}; + +int st_ism330dlc_allocate_rings(struct ism330dlc_data *cdata) +{ + int err; + struct ism330dlc_sensor_data *sdata; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_ACCEL], + NULL, &st_ism330dlc_outdata_trigger_handler, + &st_ism330dlc_buffer_setup_ops); + if (err < 0) + return err; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_GYRO]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_GYRO], + NULL, &st_ism330dlc_outdata_trigger_handler, + &st_ism330dlc_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_accel; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_TILT], + &st_ism330dlc_handler_empty, NULL, + &st_ism330dlc_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_gyro; + + return 0; + +buffer_cleanup_gyro: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +buffer_cleanup_accel: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + return err; +} + +void st_ism330dlc_deallocate_rings(struct ism330dlc_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +} diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_core.c b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_core.c new file mode 100644 index 000000000000..49a8d6f9c5f9 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_core.c @@ -0,0 +1,2810 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism330dlc core driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Denis Ciocca + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "st_ism330dlc.h" + +#define MS_TO_NS(msec) ((msec) * 1000 * 1000) + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif + +#define MIN_BNZ(a, b) (((a) < (b)) ? ((a == 0) ? \ + (b) : (a)) : ((b == 0) ? \ + (a) : (b))) + +/* COMMON VALUES FOR ACCEL-GYRO SENSORS */ +#define ST_ISM330DLC_WAI_ADDRESS 0x0f +#define ST_ISM330DLC_WAI_EXP 0x6a +#define ST_ISM330DLC_INT1_ADDR 0x0d +#define ST_ISM330DLC_INT2_ADDR 0x0e +#define ST_ISM330DLC_ACCEL_DRDY_IRQ_MASK 0x01 +#define ST_ISM330DLC_GYRO_DRDY_IRQ_MASK 0x02 +#define ST_ISM330DLC_MD1_ADDR 0x5e +#define ST_ISM330DLC_ODR_LIST_NUM 7 +#define ST_ISM330DLC_ODR_POWER_OFF_VAL 0x00 +#define ST_ISM330DLC_ODR_13HZ_VAL 0x01 +#define ST_ISM330DLC_ODR_26HZ_VAL 0x02 +#define ST_ISM330DLC_ODR_52HZ_VAL 0x03 +#define ST_ISM330DLC_ODR_104HZ_VAL 0x04 +#define ST_ISM330DLC_ODR_208HZ_VAL 0x05 +#define ST_ISM330DLC_ODR_416HZ_VAL 0x06 +#define ST_ISM330DLC_ODR_833HZ_VAL 0x07 +#define ST_ISM330DLC_FS_LIST_NUM 4 +#define ST_ISM330DLC_BDU_ADDR 0x12 +#define ST_ISM330DLC_BDU_MASK 0x40 +#define ST_ISM330DLC_EN_BIT 0x01 +#define ST_ISM330DLC_DIS_BIT 0x00 +#define ST_ISM330DLC_FUNC_EN_ADDR 0x19 +#define ST_ISM330DLC_FUNC_EN_MASK 0x04 +#define ST_ISM330DLC_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_ISM330DLC_FUNC_CFG_ACCESS_MASK 0x01 +#define ST_ISM330DLC_FUNC_CFG_ACCESS_MASK2 0x04 +#define ST_ISM330DLC_FUNC_CFG_REG2_MASK 0x80 +#define ST_ISM330DLC_FUNC_CFG_START1_ADDR 0x62 +#define ST_ISM330DLC_FUNC_CFG_START2_ADDR 0x63 +#define ST_ISM330DLC_SENSORHUB_ADDR 0x1a +#define ST_ISM330DLC_SENSORHUB_MASK 0x01 +#define ST_ISM330DLC_SENSORHUB_TRIG_MASK 0x10 +#define ST_ISM330DLC_TRIG_INTERNAL 0x00 +#define ST_ISM330DLC_TRIG_EXTERNAL 0x01 +#define ST_ISM330DLC_SELFTEST_ADDR 0x14 +#define ST_ISM330DLC_SELFTEST_ACCEL_MASK 0x03 +#define ST_ISM330DLC_SELFTEST_GYRO_MASK 0x0c +#define ST_ISM330DLC_SELF_TEST_DISABLED_VAL 0x00 +#define ST_ISM330DLC_SELF_TEST_POS_SIGN_VAL 0x01 +#define ST_ISM330DLC_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 +#define ST_ISM330DLC_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 +#define ST_ISM330DLC_LIR_ADDR 0x58 +#define ST_ISM330DLC_LIR_MASK 0x01 +#define ST_ISM330DLC_TIMER_EN_ADDR 0x19 +#define ST_ISM330DLC_TIMER_EN_MASK 0x20 +#define ST_ISM330DLC_INT2_ON_INT1_ADDR 0x13 +#define ST_ISM330DLC_INT2_ON_INT1_MASK 0x20 +#define ST_ISM330DLC_MIN_DURATION_MS 1638 +#define ST_ISM330DLC_ROUNDING_ADDR 0x16 +#define ST_ISM330DLC_ROUNDING_MASK 0x04 +#define ST_ISM330DLC_FIFO_MODE_ADDR 0x0a +#define ST_ISM330DLC_FIFO_MODE_MASK 0x07 +#define ST_ISM330DLC_FIFO_MODE_BYPASS 0x00 +#define ST_ISM330DLC_FIFO_MODE_CONTINUOS 0x06 +#define ST_ISM330DLC_FIFO_THRESHOLD_IRQ_MASK 0x08 +#define ST_ISM330DLC_FIFO_ODR_MAX 0x40 +#define ST_ISM330DLC_FIFO_DECIMATOR_ADDR 0x08 +#define ST_ISM330DLC_FIFO_ACCEL_DECIMATOR_MASK 0x07 +#define ST_ISM330DLC_FIFO_GYRO_DECIMATOR_MASK 0x38 +#define ST_ISM330DLC_FIFO_DECIMATOR2_ADDR 0x09 +#define ST_ISM330DLC_FIFO_THR_L_ADDR 0x06 +#define ST_ISM330DLC_FIFO_THR_H_ADDR 0x07 +#define ST_ISM330DLC_FIFO_THR_MASK 0x07ff +#define ST_ISM330DLC_FIFO_THR_IRQ_MASK 0x08 +#define ST_ISM330DLC_RESET_ADDR 0x12 +#define ST_ISM330DLC_RESET_MASK 0x01 +#define ST_ISM330DLC_TEST_REG_ADDR 0x00 +#define ST_ISM330DLC_START_INJECT_XL_MASK 0x08 +#define ST_ISM330DLC_INJECT_XL_X_ADDR 0x06 +#define ST_ISM330DLC_SELFTEST_NA_MS "na" +#define ST_ISM330DLC_SELFTEST_FAIL_MS "fail" +#define ST_ISM330DLC_SELFTEST_PASS_MS "pass" + +/* CUSTOM VALUES FOR ACCEL SENSOR */ +#define ST_ISM330DLC_ACCEL_ODR_ADDR 0x10 +#define ST_ISM330DLC_ACCEL_ODR_MASK 0xf0 +#define ST_ISM330DLC_ACCEL_FS_ADDR 0x10 +#define ST_ISM330DLC_ACCEL_FS_MASK 0x0c +#define ST_ISM330DLC_ACCEL_FS_2G_VAL 0x00 +#define ST_ISM330DLC_ACCEL_FS_4G_VAL 0x02 +#define ST_ISM330DLC_ACCEL_FS_8G_VAL 0x03 +#define ST_ISM330DLC_ACCEL_FS_16G_VAL 0x01 +#define ST_ISM330DLC_ACCEL_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_ISM330DLC_ACCEL_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_ISM330DLC_ACCEL_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_ISM330DLC_ACCEL_FS_16G_GAIN IIO_G_TO_M_S_2(488000) +#define ST_ISM330DLC_ACCEL_OUT_X_L_ADDR 0x28 +#define ST_ISM330DLC_ACCEL_OUT_Y_L_ADDR 0x2a +#define ST_ISM330DLC_ACCEL_OUT_Z_L_ADDR 0x2c +#define ST_ISM330DLC_ACCEL_STD_52HZ 1 +#define ST_ISM330DLC_ACCEL_STD_104HZ 2 +#define ST_ISM330DLC_ACCEL_STD_208HZ 3 +#define ST_ISM330DLC_SELFTEST_ACCEL_ADDR 0x10 +#define ST_ISM330DLC_SELFTEST_ACCEL_REG_VALUE 0x40 +#define ST_ISM330DLC_SELFTEST_ACCEL_MIN 1492 +#define ST_ISM330DLC_SELFTEST_ACCEL_MAX 27868 + +/* CUSTOM VALUES FOR GYRO SENSOR */ +#define ST_ISM330DLC_GYRO_ODR_ADDR 0x11 +#define ST_ISM330DLC_GYRO_ODR_MASK 0xf0 +#define ST_ISM330DLC_GYRO_FS_ADDR 0x11 +#define ST_ISM330DLC_GYRO_FS_MASK 0x0c +#define ST_ISM330DLC_GYRO_FS_250_VAL 0x00 +#define ST_ISM330DLC_GYRO_FS_500_VAL 0x01 +#define ST_ISM330DLC_GYRO_FS_1000_VAL 0x02 +#define ST_ISM330DLC_GYRO_FS_2000_VAL 0x03 +#define ST_ISM330DLC_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_ISM330DLC_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_ISM330DLC_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_ISM330DLC_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_ISM330DLC_GYRO_OUT_X_L_ADDR 0x22 +#define ST_ISM330DLC_GYRO_OUT_Y_L_ADDR 0x24 +#define ST_ISM330DLC_GYRO_OUT_Z_L_ADDR 0x26 +#define ST_ISM330DLC_GYRO_STD_13HZ 2 +#define ST_ISM330DLC_GYRO_STD_52HZ 3 +#define ST_ISM330DLC_GYRO_STD_104HZ 5 +#define ST_ISM330DLC_GYRO_STD_208HZ 8 +#define ST_ISM330DLC_SELFTEST_GYRO_ADDR 0x11 +#define ST_ISM330DLC_SELFTEST_GYRO_REG_VALUE 0x4c +#define ST_ISM330DLC_SELFTEST_GYRO_MIN 2142 +#define ST_ISM330DLC_SELFTEST_GYRO_MAX 10000 + +/* CUSTOM VALUES FOR TILT SENSOR */ +#define ST_ISM330DLC_TILT_EN_ADDR 0x19 +#define ST_ISM330DLC_TILT_EN_MASK 0x08 +#define ST_ISM330DLC_TILT_DRDY_IRQ_MASK 0x02 + +#define ST_ISM330DLC_ACCEL_SUFFIX_NAME "accel" +#define ST_ISM330DLC_GYRO_SUFFIX_NAME "gyro" +#define ST_ISM330DLC_TILT_SUFFIX_NAME "tilt" + +#define ST_ISM330DLC_26HZ_INJECT_NS_UP (ULLONG_MAX) +#define ST_ISM330DLC_26HZ_INJECT_NS_DOWN (25641026LL) +#define ST_ISM330DLC_52HZ_INJECT_NS_UP ST_ISM330DLC_26HZ_INJECT_NS_DOWN +#define ST_ISM330DLC_52HZ_INJECT_NS_DOWN (12820512LL) +#define ST_ISM330DLC_104HZ_INJECT_NS_UP ST_ISM330DLC_52HZ_INJECT_NS_DOWN +#define ST_ISM330DLC_104HZ_INJECT_NS_DOWN (6410256LL) +#define ST_ISM330DLC_208HZ_INJECT_NS_UP ST_ISM330DLC_104HZ_INJECT_NS_DOWN +#define ST_ISM330DLC_208HZ_INJECT_NS_DOWN (0) + +#define ST_ISM330DLC_DEV_ATTR_SAMP_FREQ() \ + IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \ + st_ism330dlc_sysfs_get_sampling_frequency, \ + st_ism330dlc_sysfs_set_sampling_frequency) + +#define ST_ISM330DLC_DEV_ATTR_SAMP_FREQ_AVAIL() \ + IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \ + st_ism330dlc_sysfs_sampling_frequency_avail) + +#define ST_ISM330DLC_DEV_ATTR_SCALE_AVAIL(name) \ + IIO_DEVICE_ATTR(name, S_IRUGO, \ + st_ism330dlc_sysfs_scale_avail, NULL , 0); + +static struct st_ism330dlc_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_ism330dlc_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_ISM330DLC_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_ISM330DLC_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_ISM330DLC_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_ISM330DLC_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_ISM330DLC_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_ISM330DLC_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +struct st_ism330dlc_odr_reg { + unsigned int hz; + u8 value; +}; + +static struct st_ism330dlc_odr_table { + u8 addr[2]; + u8 mask[2]; + struct st_ism330dlc_odr_reg odr_avl[ST_ISM330DLC_ODR_LIST_NUM]; +} st_ism330dlc_odr_table = { + .addr[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_ODR_ADDR, + .mask[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_ODR_MASK, + .addr[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_ODR_ADDR, + .mask[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_ODR_MASK, + .odr_avl[0] = { .hz = 13, .value = ST_ISM330DLC_ODR_13HZ_VAL }, + .odr_avl[1] = { .hz = 26, .value = ST_ISM330DLC_ODR_26HZ_VAL }, + .odr_avl[2] = { .hz = 52, .value = ST_ISM330DLC_ODR_52HZ_VAL }, + .odr_avl[3] = { .hz = 104, .value = ST_ISM330DLC_ODR_104HZ_VAL }, + .odr_avl[4] = { .hz = 208, .value = ST_ISM330DLC_ODR_208HZ_VAL }, + .odr_avl[5] = { .hz = 416, .value = ST_ISM330DLC_ODR_416HZ_VAL }, + .odr_avl[6] = { .hz = 833, .value = ST_ISM330DLC_ODR_833HZ_VAL }, +}; + +struct st_ism330dlc_fs_reg { + unsigned int gain; + u8 value; +}; + +static struct st_ism330dlc_fs_table { + u8 addr; + u8 mask; + struct st_ism330dlc_fs_reg fs_avl[ST_ISM330DLC_FS_LIST_NUM]; +} st_ism330dlc_fs_table[ST_INDIO_DEV_NUM] = { + [ST_MASK_ID_ACCEL] = { + .addr = ST_ISM330DLC_ACCEL_FS_ADDR, + .mask = ST_ISM330DLC_ACCEL_FS_MASK, + .fs_avl[0] = { .gain = ST_ISM330DLC_ACCEL_FS_2G_GAIN, + .value = ST_ISM330DLC_ACCEL_FS_2G_VAL }, + .fs_avl[1] = { .gain = ST_ISM330DLC_ACCEL_FS_4G_GAIN, + .value = ST_ISM330DLC_ACCEL_FS_4G_VAL }, + .fs_avl[2] = { .gain = ST_ISM330DLC_ACCEL_FS_8G_GAIN, + .value = ST_ISM330DLC_ACCEL_FS_8G_VAL }, + .fs_avl[3] = { .gain = ST_ISM330DLC_ACCEL_FS_16G_GAIN, + .value = ST_ISM330DLC_ACCEL_FS_16G_VAL }, + }, + [ST_MASK_ID_GYRO] = { + .addr = ST_ISM330DLC_GYRO_FS_ADDR, + .mask = ST_ISM330DLC_GYRO_FS_MASK, + .fs_avl[0] = { .gain = ST_ISM330DLC_GYRO_FS_250_GAIN, + .value = ST_ISM330DLC_GYRO_FS_250_VAL }, + .fs_avl[1] = { .gain = ST_ISM330DLC_GYRO_FS_500_GAIN, + .value = ST_ISM330DLC_GYRO_FS_500_VAL }, + .fs_avl[2] = { .gain = ST_ISM330DLC_GYRO_FS_1000_GAIN, + .value = ST_ISM330DLC_GYRO_FS_1000_VAL }, + .fs_avl[3] = { .gain = ST_ISM330DLC_GYRO_FS_2000_GAIN, + .value = ST_ISM330DLC_GYRO_FS_2000_VAL }, + } +}; + +static const struct iio_event_spec singol_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, +}; + +const struct iio_event_spec ism330dlc_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_ism330dlc_accel_ch[] = { + ST_ISM330DLC_LSM_CHANNELS(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_ISM330DLC_ACCEL_OUT_X_L_ADDR, 's'), + ST_ISM330DLC_LSM_CHANNELS(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_ISM330DLC_ACCEL_OUT_Y_L_ADDR, 's'), + ST_ISM330DLC_LSM_CHANNELS(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_ISM330DLC_ACCEL_OUT_Z_L_ADDR, 's'), + ST_ISM330DLC_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_ism330dlc_gyro_ch[] = { + ST_ISM330DLC_LSM_CHANNELS(IIO_ANGL_VEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_ISM330DLC_GYRO_OUT_X_L_ADDR, 's'), + ST_ISM330DLC_LSM_CHANNELS(IIO_ANGL_VEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_ISM330DLC_GYRO_OUT_Y_L_ADDR, 's'), + ST_ISM330DLC_LSM_CHANNELS(IIO_ANGL_VEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_ISM330DLC_GYRO_OUT_Z_L_ADDR, 's'), + ST_ISM330DLC_FLUSH_CHANNEL(IIO_ANGL_VEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_ism330dlc_tilt_ch[] = { + ST_ISM330DLC_FLUSH_CHANNEL(IIO_TILT), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + + +int st_ism330dlc_write_data_with_mask(struct ism330dlc_data *cdata, + u8 reg_addr, u8 mask, u8 data, + bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = cdata->tf->read(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} +EXPORT_SYMBOL(st_ism330dlc_write_data_with_mask); + +static inline int st_ism330dlc_enable_embedded_page_regs(struct ism330dlc_data *cdata, + bool enable) +{ + u8 value = 0x00; + + if (enable) + value = ST_ISM330DLC_FUNC_CFG_REG2_MASK; + + return cdata->tf->write(cdata, ST_ISM330DLC_FUNC_CFG_ACCESS_ADDR, + 1, &value, false); +} + +int st_ism330dlc_write_embedded_registers(struct ism330dlc_data *cdata, + u8 reg_addr, u8 *data, int len) +{ + int err = 0, err2, count = 0; + + mutex_lock(&cdata->bank_registers_lock); + + if (cdata->enable_digfunc_mask) { + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_FUNC_EN_ADDR, + ST_ISM330DLC_FUNC_EN_MASK, + ST_ISM330DLC_DIS_BIT, false); + if (err < 0) { + mutex_unlock(&cdata->bank_registers_lock); + return err; + } + } + + udelay(100); + + err = st_ism330dlc_enable_embedded_page_regs(cdata, true); + if (err < 0) + goto restore_digfunc; + + udelay(100); + + err = cdata->tf->write(cdata, reg_addr, len, data, false); + if (err < 0) + goto restore_bank_regs; + + err = st_ism330dlc_enable_embedded_page_regs(cdata, false); + if (err < 0) + goto restore_bank_regs; + + udelay(100); + + if (cdata->enable_digfunc_mask) { + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_FUNC_EN_ADDR, + ST_ISM330DLC_FUNC_EN_MASK, + ST_ISM330DLC_EN_BIT, false); + if (err < 0) + goto restore_digfunc; + } + + mutex_unlock(&cdata->bank_registers_lock); + + return 0; + +restore_bank_regs: + do { + msleep(200); + err2 = st_ism330dlc_enable_embedded_page_regs(cdata, false); + } while ((err2 < 0) && (count++ < 10)); + + if (count >= 10) + pr_err("not able to close embedded page registers. It make driver unstable!\n"); + +restore_digfunc: + if (!cdata->enable_digfunc_mask) { + err2 = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_FUNC_EN_ADDR, + ST_ISM330DLC_FUNC_EN_MASK, + ST_ISM330DLC_EN_BIT, false); + } + + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int ism330dlc_set_watermark(struct ism330dlc_data *cdata) +{ + int err; + u8 reg_value = 0; + u16 fifo_watermark; + unsigned int fifo_len, sip = 0, min_pattern = UINT_MAX; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_ACCEL] / + cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + } + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_GYRO].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_GYRO] / + cdata->fifo_output[ST_MASK_ID_GYRO].sip); + } + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_EXT0].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_EXT0] / + cdata->fifo_output[ST_MASK_ID_EXT0].sip); + } +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + if (sip == 0) + return 0; + + if (min_pattern == 0) + min_pattern = 1; + + min_pattern = MIN(min_pattern, + ((unsigned int)ST_ISM330DLC_MAX_FIFO_THRESHOLD / sip)); + + fifo_len = min_pattern * sip * ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE; + fifo_watermark = (fifo_len / 2); + + if (fifo_watermark < (ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE / 2)) + fifo_watermark = ST_ISM330DLC_FIFO_ELEMENT_LEN_BYTE / 2; + + if (fifo_watermark != cdata->fifo_watermark) { + err = cdata->tf->read(cdata, ST_ISM330DLC_FIFO_THR_H_ADDR, 1, + ®_value, true); + if (err < 0) + return err; + + fifo_watermark = (fifo_watermark & ST_ISM330DLC_FIFO_THR_MASK) | + ((reg_value & ~ST_ISM330DLC_FIFO_THR_MASK) << 8); + + err = cdata->tf->write(cdata, ST_ISM330DLC_FIFO_THR_L_ADDR, 2, + (u8 *)&fifo_watermark, true); + if (err < 0) + return err; + + cdata->fifo_watermark = fifo_watermark; + } + + return 0; +} + +int st_ism330dlc_set_fifo_mode(struct ism330dlc_data *cdata, enum fifo_mode fm) +{ + int err; + u8 reg_value; + + switch (fm) { + case BYPASS: + reg_value = ST_ISM330DLC_FIFO_MODE_BYPASS; + break; + case CONTINUOS: + reg_value = ST_ISM330DLC_FIFO_MODE_CONTINUOS | ST_ISM330DLC_FIFO_ODR_MAX; + break; + default: + return -EINVAL; + } + + err = cdata->tf->write(cdata, ST_ISM330DLC_FIFO_MODE_ADDR, 1, + ®_value, true); + if (err < 0) + return err; + + if (fm != BYPASS) { + cdata->slower_counter = 0; + cdata->fifo_enable_timestamp = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = 0; + } + + cdata->fifo_status = fm; + + return 0; +} +EXPORT_SYMBOL(st_ism330dlc_set_fifo_mode); + +static int ism330dlc_write_decimators(struct ism330dlc_data *cdata, + u8 decimators[3]) +{ + int i; + u8 value[3], decimators_reg[2]; + + for (i = 0; i < 3; i++) { + switch (decimators[i]) { + case 0: + case 1: + case 2: + case 3: + case 4: + value[i] = decimators[i]; + break; + case 8: + value[i] = 0x05; + break; + case 16: + value[i] = 0x06; + break; + case 32: + value[i] = 0x07; + break; + default: + return -EINVAL; + } + } + + decimators_reg[0] = value[0] | (value[1] << 3); + decimators_reg[1] = value[2]; + + return cdata->tf->write(cdata, ST_ISM330DLC_FIFO_DECIMATOR_ADDR, + ARRAY_SIZE(decimators_reg), decimators_reg, true); +} + +static bool ism330dlc_calculate_fifo_decimators(struct ism330dlc_data *cdata, + u8 decimators[3], u8 samples_in_pattern[3], + unsigned int new_v_odr[ST_INDIO_DEV_NUM + 1], + unsigned int new_hw_odr[ST_INDIO_DEV_NUM + 1], + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1], + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1]) +{ + unsigned int trigger_odr; + u8 min_decimator, max_decimator = 0; + u8 accel_decimator = 0, gyro_decimator = 0, ext_decimator = 0; + + trigger_odr = new_hw_odr[ST_MASK_ID_ACCEL]; + if (trigger_odr < new_hw_odr[ST_MASK_ID_GYRO]) + trigger_odr = new_hw_odr[ST_MASK_ID_GYRO]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_ACCEL)) && + (new_v_odr[ST_MASK_ID_ACCEL] != 0) && cdata->accel_on) + accel_decimator = trigger_odr / new_v_odr[ST_MASK_ID_ACCEL]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_GYRO)) && + (new_v_odr[ST_MASK_ID_GYRO] != 0) && + (new_hw_odr[ST_MASK_ID_GYRO] > 0)) + gyro_decimator = trigger_odr / new_v_odr[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_EXT0)) && + (new_v_odr[ST_MASK_ID_EXT0] != 0) && cdata->magn_on) + ext_decimator = trigger_odr / new_v_odr[ST_MASK_ID_EXT0]; + + new_fifo_decimator[ST_MASK_ID_EXT0] = 1; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + new_fifo_decimator[ST_MASK_ID_ACCEL] = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = 1; + + if ((accel_decimator != 0) || (gyro_decimator != 0) || (ext_decimator != 0)) { + min_decimator = MIN_BNZ(MIN_BNZ(accel_decimator, gyro_decimator), ext_decimator); + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + if (min_decimator != 1) { + if ((accel_decimator / min_decimator) == 1) { + accel_decimator = 1; + new_fifo_decimator[ST_MASK_ID_ACCEL] = min_decimator; + } else if ((gyro_decimator / min_decimator) == 1) { + gyro_decimator = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = min_decimator; + } else if ((ext_decimator / min_decimator) == 1) { + ext_decimator = 1; + new_fifo_decimator[ST_MASK_ID_EXT0] = min_decimator; + } + min_decimator = 1; + } + if ((accel_decimator > 4) && (accel_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 3; + accel_decimator = 4; + } else if ((accel_decimator > 8) && (accel_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 7; + accel_decimator = 8; + } else if (accel_decimator > 16 && accel_decimator < 32) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 15; + accel_decimator = 16; + } else if (accel_decimator > 32) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator / 32; + accel_decimator = 32; + } + if ((gyro_decimator > 4) && (gyro_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 3; + gyro_decimator = 4; + } else if ((gyro_decimator > 8) && (gyro_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 7; + gyro_decimator = 8; + } else if (gyro_decimator > 16 && gyro_decimator < 32) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 15; + gyro_decimator = 16; + } else if (gyro_decimator > 32) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator / 32; + gyro_decimator = 32; + } +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if ((ext_decimator > 4) && (ext_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 3; + ext_decimator = 4; + } else if ((ext_decimator > 8) && (ext_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 7; + ext_decimator = 8; + } else if (ext_decimator > 16 && ext_decimator < 32) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 15; + ext_decimator = 16; + } else if (ext_decimator > 32) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator / 32; + ext_decimator = 32; + } +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + } + + decimators[0] = accel_decimator; + if (accel_decimator > 0) { + new_deltatime[ST_MASK_ID_ACCEL] = accel_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[0] = max_decimator / accel_decimator; + } else + samples_in_pattern[0] = 0; + + decimators[1] = gyro_decimator; + if (gyro_decimator > 0) { + new_deltatime[ST_MASK_ID_GYRO] = gyro_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[1] = max_decimator / gyro_decimator; + } else + samples_in_pattern[1] = 0; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + decimators[2] = ext_decimator; + if (ext_decimator > 0) { + new_deltatime[ST_MASK_ID_EXT0] = ext_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[2] = max_decimator / ext_decimator; + } else + samples_in_pattern[2] = 0; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (ext_decimator == cdata->hwfifo_decimator[ST_MASK_ID_EXT0]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#else /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + return false; + } + + return true; +} + +static int st_ism330dlc_of_get_drdy_pin(struct ism330dlc_data *cdata, + int *drdy_pin) +{ + struct device_node *np = cdata->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin); +} + +static int st_ism330dlc_get_drdy_reg(struct ism330dlc_data *cdata, u8 *drdy_reg) +{ + int err = 0, drdy_pin; + + if (st_ism330dlc_of_get_drdy_pin(cdata, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = cdata->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + *drdy_reg = ST_ISM330DLC_INT1_ADDR; + break; + case 2: + *drdy_reg = ST_ISM330DLC_INT2_ADDR; + break; + default: + dev_err(cdata->dev, "unsupported data ready pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +int st_ism330dlc_set_drdy_irq(struct ism330dlc_sensor_data *sdata, bool state) +{ + int err; + u16 *irq_mask = NULL; + u8 reg_addr, mask = 0, value; + u16 tmp_irq_enable_fifo_mask, tmp_irq_enable_accel_ext_mask; + + if (state) + value = ST_ISM330DLC_EN_BIT; + else + value = ST_ISM330DLC_DIS_BIT; + + tmp_irq_enable_fifo_mask = + sdata->cdata->irq_enable_fifo_mask & ~sdata->sindex; + tmp_irq_enable_accel_ext_mask = + sdata->cdata->irq_enable_accel_ext_mask & ~sdata->sindex; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = sdata->cdata->drdy_reg; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_ACCEL]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_ISM330DLC_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_ISM330DLC_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; + case ST_MASK_ID_GYRO: + reg_addr = sdata->cdata->drdy_reg; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_GYRO]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_ISM330DLC_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else + mask = ST_ISM330DLC_GYRO_DRDY_IRQ_MASK; + + break; + case ST_MASK_ID_TILT: + reg_addr = ST_ISM330DLC_MD1_ADDR; + mask = ST_ISM330DLC_TILT_DRDY_IRQ_MASK; + break; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + reg_addr = sdata->cdata->drdy_reg; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_ISM330DLC_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_ISM330DLC_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + default: + return -EINVAL; + } + + if (mask > 0) { + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); + if (err < 0) + return err; + } + + if (irq_mask != NULL) { + if (state) + *irq_mask |= BIT(sdata->sindex); + else + *irq_mask &= ~BIT(sdata->sindex); + } + + return 0; +} +EXPORT_SYMBOL(st_ism330dlc_set_drdy_irq); + +static int st_ism330dlc_set_odr(struct ism330dlc_sensor_data *sdata, + unsigned int odr, bool force) +{ + u8 reg_value; + int err, i = 0, n; + int64_t temp_last_timestamp[3] = { 0 }; + bool scan_odr = true, fifo_conf_changed; + unsigned int temp_v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int temp_hw_odr[ST_INDIO_DEV_NUM + 1]; + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1] = { 0 }; + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1] = { 0 }; + u8 fifo_decimator[3] = { 0 }, samples_in_pattern[3] = { 0 }; + u8 temp_num_samples[3] = { 0 }, temp_old_decimator[3] = { 1 }; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + + if (scan_odr) { + for (i = 0; i < ST_ISM330DLC_ODR_LIST_NUM; i++) { + if (st_ism330dlc_odr_table.odr_avl[i].hz == odr) + break; + } + if (i == ST_ISM330DLC_ODR_LIST_NUM) + return -EINVAL; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = st_ism330dlc_odr_table.odr_avl[i].hz; + return 0; + } + } + + if (sdata->cdata->hw_odr[sdata->sindex] == st_ism330dlc_odr_table.odr_avl[i].hz) + reg_value = 0xff; + else + reg_value = st_ism330dlc_odr_table.odr_avl[i].value; + } else + reg_value = ST_ISM330DLC_ODR_POWER_OFF_VAL; + + if (sdata->cdata->sensors_use_fifo > 0) { + /* someone is using fifo */ + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + temp_v_odr[ST_MASK_ID_GYRO] = sdata->cdata->v_odr[ST_MASK_ID_GYRO]; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (force) + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->accel_odr_dependency[0]; + + temp_hw_odr[ST_MASK_ID_ACCEL] = odr; + temp_hw_odr[ST_MASK_ID_GYRO] = sdata->cdata->hw_odr[ST_MASK_ID_GYRO]; + } else { + if (!force) + temp_v_odr[ST_MASK_ID_GYRO] = odr; + + temp_hw_odr[ST_MASK_ID_GYRO] = odr; + temp_hw_odr[ST_MASK_ID_ACCEL] = sdata->cdata->hw_odr[ST_MASK_ID_ACCEL]; + } +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + temp_v_odr[ST_MASK_ID_EXT0] = sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + fifo_conf_changed = ism330dlc_calculate_fifo_decimators(sdata->cdata, + fifo_decimator, samples_in_pattern, temp_v_odr, + temp_hw_odr, new_deltatime, new_fifo_decimator); + if (fifo_conf_changed) { + /* FIFO configuration changed, needs to write new decimators */ + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_ism330dlc_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + err = st_ism330dlc_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } + + err = ism330dlc_write_decimators(sdata->cdata, fifo_decimator); + if (err < 0) + goto reenable_fifo_irq; + + if (reg_value != 0xff) { + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + st_ism330dlc_odr_table.addr[sdata->sindex], + st_ism330dlc_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_208HZ; + break; + } + } + + sdata->cdata->hwfifo_decimator[ST_MASK_ID_ACCEL] = fifo_decimator[0]; + sdata->cdata->hwfifo_decimator[ST_MASK_ID_GYRO] = fifo_decimator[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator = new_fifo_decimator[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator = new_fifo_decimator[ST_MASK_ID_GYRO]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = new_fifo_decimator[ST_MASK_ID_ACCEL] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = new_fifo_decimator[ST_MASK_ID_GYRO] - 1; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip = samples_in_pattern[0]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip = samples_in_pattern[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default = new_deltatime[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + sdata->cdata->hwfifo_decimator[ST_MASK_ID_EXT0] = fifo_decimator[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator = new_fifo_decimator[ST_MASK_ID_EXT0]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = new_fifo_decimator[ST_MASK_ID_EXT0] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip = samples_in_pattern[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + err = ism330dlc_set_watermark(sdata->cdata); + if (err < 0) + goto reenable_fifo_irq; + + if ((samples_in_pattern[0] > 0) || (samples_in_pattern[1] > 0) || (samples_in_pattern[2] > 0)) { + err = st_ism330dlc_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_ism330dlc_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_ism330dlc_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_ism330dlc_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } else { + /* FIFO configuration not changed */ + + if (reg_value == 0xff) { + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + return 0; + } + + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_ism330dlc_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + err = st_ism330dlc_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } + + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + st_ism330dlc_odr_table.addr[sdata->sindex], + st_ism330dlc_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_208HZ; + break; + } + + if ((sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0)) { + err = st_ism330dlc_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_ism330dlc_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_ism330dlc_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_ism330dlc_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + } else { + /* no one is using FIFO */ + + disable_irq(sdata->cdata->irq); + + if ((odr != 0) && (sdata->cdata->hw_odr[sdata->sindex] == st_ism330dlc_odr_table.odr_avl[i].hz)) { + if (sdata->sindex == ST_MASK_ID_ACCEL) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + + return 0; + } + + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + st_ism330dlc_odr_table.addr[sdata->sindex], + st_ism330dlc_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) { + enable_irq(sdata->cdata->irq); + return err; + } + + if (!force) + sdata->cdata->v_odr[sdata->sindex] = st_ism330dlc_odr_table.odr_avl[i].hz; + + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = st_ism330dlc_odr_table.odr_avl[i].hz; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (sdata->cdata->hw_odr[sdata->sindex]) { + case 13: + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_ISM330DLC_ACCEL_STD_208HZ; + break; + } + } + + switch (sdata->cdata->hw_odr[ST_MASK_ID_GYRO]) { + case 13: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_13HZ; + break; + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_ISM330DLC_GYRO_STD_208HZ; + break; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (sdata->cdata->hw_odr[sdata->sindex] > 0) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } else { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = 1; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = 1; + } + + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + sdata->cdata->trigger_odr = sdata->cdata->hw_odr[0] > sdata->cdata->hw_odr[1] ? sdata->cdata->hw_odr[0] : sdata->cdata->hw_odr[1]; + + return 0; + +reenable_fifo_irq: + enable_irq(sdata->cdata->irq); + return err; +} + +/* + * Enable / disable accelerometer + */ +static int ism330dlc_enable_accel(struct ism330dlc_data *cdata, enum st_mask_id id, int min_odr) +{ + int odr; + struct ism330dlc_sensor_data *sdata_accel = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + switch (id) { + case ST_MASK_ID_ACCEL: + cdata->accel_odr_dependency[0] = min_odr; + if (min_odr > 0) + cdata->accel_on = true; + else + cdata->accel_on = false; + + break; + case ST_MASK_ID_SENSOR_HUB: + cdata->accel_odr_dependency[1] = min_odr; + if (min_odr > 0) + cdata->magn_on = true; + else + cdata->magn_on = false; + + break; + case ST_MASK_ID_DIGITAL_FUNC: + cdata->accel_odr_dependency[2] = min_odr; + break; + default: + return -EINVAL; + } + + if (cdata->accel_odr_dependency[0] > cdata->accel_odr_dependency[1]) + odr = cdata->accel_odr_dependency[0]; + else + odr = cdata->accel_odr_dependency[1]; + + if (cdata->accel_odr_dependency[2] > odr) + odr = cdata->accel_odr_dependency[2]; + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + if (cdata->injection_mode) + return 0; +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + + return st_ism330dlc_set_odr(sdata_accel, odr, true); +} + +/* + * Enable / disable digital func + */ +static int ism330dlc_enable_digital_func(struct ism330dlc_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_digfunc_mask == 0) { + err = ism330dlc_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 26); + if (err < 0) + return err; + + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_FUNC_EN_ADDR, + ST_ISM330DLC_FUNC_EN_MASK, + ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask |= BIT(id); + } else { + if ((cdata->enable_digfunc_mask & ~BIT(id)) == 0) { + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_FUNC_EN_ADDR, + ST_ISM330DLC_FUNC_EN_MASK, + ST_ISM330DLC_DIS_BIT, true); + if (err < 0) + return err; + + err = ism330dlc_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 0); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask &= ~BIT(id); + + } + + return 0; +} + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT +int st_ism330dlc_enable_sensor_hub(struct ism330dlc_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_sensorhub_mask == 0) { + err = ism330dlc_enable_digital_func(cdata, + true, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + + err = ism330dlc_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + if (err < 0) + return err; + + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_SENSORHUB_ADDR, + ST_ISM330DLC_SENSORHUB_MASK, + ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; + + } else + err = ism330dlc_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask |= BIT(id); + } else { + if ((cdata->enable_sensorhub_mask & ~BIT(id)) == 0) { + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_SENSORHUB_ADDR, + ST_ISM330DLC_SENSORHUB_MASK, + ST_ISM330DLC_DIS_BIT, true); + if (err < 0) + return err; + + err = ism330dlc_enable_accel(cdata, + ST_MASK_ID_SENSOR_HUB, 0); + if (err < 0) + return err; + + err = ism330dlc_enable_digital_func(cdata, + false, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + } else + err = ism330dlc_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask &= ~BIT(id); + } + + return err < 0 ? err : 0; +} +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + +int st_ism330dlc_set_enable(struct ism330dlc_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + err = ism330dlc_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, + enable ? sdata->cdata->v_odr[ST_MASK_ID_ACCEL] : 0); + if (err < 0) + return 0; + + break; + case ST_MASK_ID_GYRO: + err = st_ism330dlc_set_odr(sdata, enable ? + sdata->cdata->v_odr[ST_MASK_ID_GYRO] : 0, true); + if (err < 0) + return err; + + break; + case ST_MASK_ID_TILT: + if (enable) + reg_value = ST_ISM330DLC_EN_BIT; + else + reg_value = ST_ISM330DLC_DIS_BIT; + + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_TILT_EN_ADDR, + ST_ISM330DLC_TILT_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = ism330dlc_enable_digital_func(sdata->cdata, + enable, ST_MASK_ID_TILT); + if (err < 0) + return err; + + break; + default: + return -EINVAL; + } + + if (buffer) { + err = st_ism330dlc_set_drdy_irq(sdata, enable); + if (err < 0) + return err; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; +} + +static int st_ism330dlc_set_fs(struct ism330dlc_sensor_data *sdata, + unsigned int gain) +{ + int err, i; + + for (i = 0; i < ST_ISM330DLC_FS_LIST_NUM; i++) { + if (st_ism330dlc_fs_table[sdata->sindex].fs_avl[i].gain == gain) + break; + } + if (i == ST_ISM330DLC_FS_LIST_NUM) + return -EINVAL; + + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + st_ism330dlc_fs_table[sdata->sindex].addr, + st_ism330dlc_fs_table[sdata->sindex].mask, + st_ism330dlc_fs_table[sdata->sindex].fs_avl[i].value, + true); + if (err < 0) + return err; + + sdata->c_gain[0] = gain; + + return 0; +} + +static int st_ism330dlc_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[ST_ISM330DLC_BYTE_FOR_CHANNEL]; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_ism330dlc_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) + msleep(40); + + if (sdata->sindex == ST_MASK_ID_GYRO) + msleep(120); + + err = sdata->cdata->tf->read(sdata->cdata, ch->address, + ST_ISM330DLC_BYTE_FOR_CHANNEL, outdata, true); + if (err < 0) { + st_ism330dlc_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + st_ism330dlc_set_enable(sdata, false, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[0]; + return IIO_VAL_INT_PLUS_NANO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_ism330dlc_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + int err; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = st_ism330dlc_set_fs(sdata, val2); + mutex_unlock(&indio_dev->mlock); + break; + default: + return -EINVAL; + } + + return err < 0 ? err : 0; +} + +static int st_ism330dlc_init_sensor(struct ism330dlc_data *cdata) +{ + int err; + u8 default_reg_value = ST_ISM330DLC_RESET_MASK; + + err = cdata->tf->write(cdata, ST_ISM330DLC_RESET_ADDR, 1, + &default_reg_value, true); + if (err < 0) + return err; + + msleep(200); + + /* Latch interrupts */ + err = st_ism330dlc_write_data_with_mask(cdata, ST_ISM330DLC_LIR_ADDR, + ST_ISM330DLC_LIR_MASK, ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; + + /* Enable BDU for sensors data */ + err = st_ism330dlc_write_data_with_mask(cdata, ST_ISM330DLC_BDU_ADDR, + ST_ISM330DLC_BDU_MASK, ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; + + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_ROUNDING_ADDR, + ST_ISM330DLC_ROUNDING_MASK, + ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; + + /* Redirect INT2 on INT1, all interrupt will be available on INT1 */ + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_INT2_ON_INT1_ADDR, + ST_ISM330DLC_INT2_ON_INT1_MASK, + ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; + + return st_ism330dlc_get_drdy_reg(cdata, &cdata->drdy_reg); +} + +static int st_ism330dlc_set_selftest(struct ism330dlc_sensor_data *sdata, int index) +{ + u8 mode, mask; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + mask = ST_ISM330DLC_SELFTEST_ACCEL_MASK; + mode = st_ism330dlc_selftest_table[index].accel_value; + break; + case ST_MASK_ID_GYRO: + mask = ST_ISM330DLC_SELFTEST_GYRO_MASK; + mode = st_ism330dlc_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_ism330dlc_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_SELFTEST_ADDR, mask, mode, true); +} + +static ssize_t st_ism330dlc_sysfs_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct ism330dlc_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_ism330dlc_sysfs_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + + mutex_lock(&sdata->cdata->odr_lock); +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + if (!((sdata->sindex & ST_MASK_ID_ACCEL) && + sdata->cdata->injection_mode)) { + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_ism330dlc_set_odr(sdata, odr, false); + } +#else /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + if (sdata->cdata->v_odr[sdata->sindex] != odr) { + if ((sdata->sindex == ST_MASK_ID_ACCEL) && + (sdata->cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL))) + err = ism330dlc_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, odr); + else + err = st_ism330dlc_set_odr(sdata, odr, false); + } +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + mutex_unlock(&sdata->cdata->odr_lock); + + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static ssize_t st_ism330dlc_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < ST_ISM330DLC_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_ism330dlc_odr_table.odr_avl[i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_ism330dlc_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + struct ism330dlc_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + for (i = 0; i < ST_ISM330DLC_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_ism330dlc_fs_table[sdata->sindex].fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_ism330dlc_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_ism330dlc_selftest_table[1].string_mode, + st_ism330dlc_selftest_table[2].string_mode); +} + +static ssize_t st_ism330dlc_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + result = sdata->cdata->accel_selftest_status; + break; + case ST_MASK_ID_GYRO: + result = sdata->cdata->gyro_selftest_status; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_ISM330DLC_SELFTEST_NA_MS; + else if (result < 0) + message = ST_ISM330DLC_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_ISM330DLC_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_ism330dlc_sysfs_start_selftest_status(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i, n; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + u8 reg_status, reg_addr, temp_reg_status, outdata[6]; + int x = 0, y = 0, z = 0, x_selftest = 0, y_selftest = 0, z_selftest = 0; + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + sdata->cdata->accel_selftest_status = 0; + break; + case ST_MASK_ID_GYRO: + sdata->cdata->gyro_selftest_status = 0; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + for (n = 0; n < ARRAY_SIZE(st_ism330dlc_selftest_table); n++) { + if (strncmp(buf, st_ism330dlc_selftest_table[n].string_mode, + size - 2) == 0) + break; + } + if (n == ARRAY_SIZE(st_ism330dlc_selftest_table)) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = ST_ISM330DLC_SELFTEST_ACCEL_ADDR; + temp_reg_status = ST_ISM330DLC_SELFTEST_ACCEL_REG_VALUE; + break; + case ST_MASK_ID_GYRO: + reg_addr = ST_ISM330DLC_SELFTEST_GYRO_ADDR; + temp_reg_status = ST_ISM330DLC_SELFTEST_GYRO_REG_VALUE; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = sdata->cdata->tf->read(sdata->cdata, + reg_addr, 1, ®_status, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, &temp_reg_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 20; + y += ((s16)*(u16 *)&outdata[2]) / 20; + z += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = st_ism330dlc_set_selftest(sdata, n); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest enabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 20; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 20; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, ®_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = st_ism330dlc_set_selftest(sdata, 0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + if ((abs(x_selftest - x) < ST_ISM330DLC_SELFTEST_ACCEL_MIN) || + (abs(x_selftest - x) > ST_ISM330DLC_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_ISM330DLC_SELFTEST_ACCEL_MIN) || + (abs(y_selftest - y) > ST_ISM330DLC_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_ISM330DLC_SELFTEST_ACCEL_MIN) || + (abs(z_selftest - z) > ST_ISM330DLC_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->accel_selftest_status = 1; + break; + case ST_MASK_ID_GYRO: + if ((abs(x_selftest - x) < ST_ISM330DLC_SELFTEST_GYRO_MIN) || + (abs(x_selftest - x) > ST_ISM330DLC_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_ISM330DLC_SELFTEST_GYRO_MIN) || + (abs(y_selftest - y) > ST_ISM330DLC_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_ISM330DLC_SELFTEST_GYRO_MIN) || + (abs(z_selftest - z) > ST_ISM330DLC_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->gyro_selftest_status = 1; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + +selftest_failure: + mutex_unlock(&sdata->cdata->odr_lock); + + return size; +} + +ssize_t st_ism330dlc_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u64 sensor_last_timestamp, event_type = 0; + int stype = 0; + u64 timestamp_flush = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_lock(&sdata->cdata->odr_lock); + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = + sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + st_ism330dlc_read_fifo(sdata->cdata, true); + + if (sensor_last_timestamp == + sdata->cdata->fifo_output[sdata->sindex].timestamp_p) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + timestamp_flush = sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + enable_irq(sdata->cdata->irq); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + stype = IIO_ACCEL; + break; + + case ST_MASK_ID_GYRO: + stype = IIO_ANGL_VEL; + break; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + stype = IIO_MAGN; + break; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + } + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(stype, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + timestamp_flush); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +ssize_t st_ism330dlc_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_enabled[sdata->sindex]); +} + +ssize_t st_ism330dlc_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + bool enable = false; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + err = -EBUSY; + goto set_hwfifo_enabled_unlock_mutex; + } + + err = strtobool(buf, &enable); + if (err < 0) + goto set_hwfifo_enabled_unlock_mutex; + + mutex_lock(&sdata->cdata->odr_lock); + + sdata->cdata->hwfifo_enabled[sdata->sindex] = enable; + + if (enable) + sdata->cdata->sensors_use_fifo |= BIT(sdata->sindex); + else + sdata->cdata->sensors_use_fifo &= ~BIT(sdata->sindex); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; + +set_hwfifo_enabled_unlock_mutex: + mutex_unlock(&indio_dev->mlock); + return err; +} + +ssize_t st_ism330dlc_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_watermark[sdata->sindex]); +} + +ssize_t st_ism330dlc_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err = 0, watermark = 0, old_watermark; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > ST_ISM330DLC_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) && + (sdata->cdata->sensors_use_fifo & BIT(sdata->sindex))) { + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) + st_ism330dlc_read_fifo(sdata->cdata, true); + + old_watermark = sdata->cdata->hwfifo_watermark[sdata->sindex]; + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + err = ism330dlc_set_watermark(sdata->cdata); + if (err < 0) + sdata->cdata->hwfifo_watermark[sdata->sindex] = old_watermark; + + enable_irq(sdata->cdata->irq); + } else + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : size; +} + +ssize_t st_ism330dlc_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", ST_ISM330DLC_MAX_FIFO_LENGHT); +} + +ssize_t st_ism330dlc_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION +static ssize_t st_ism330dlc_sysfs_set_injection_mode(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, start; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = kstrtoint(buf, 10, &start); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + mutex_lock(&sdata->cdata->odr_lock); + + if (start == 0) { + /* End injection */ + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_TEST_REG_ADDR, + ST_ISM330DLC_START_INJECT_XL_MASK, 0, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + /* Force accel ODR to 26Hz if dependencies are enabled */ + if (sdata->cdata->sensors_enabled > 0) { + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + st_ism330dlc_odr_table.addr[sdata->sindex], + st_ism330dlc_odr_table.mask[sdata->sindex], + st_ism330dlc_odr_table.odr_avl[1].value, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + } + + sdata->cdata->injection_mode = false; + } else { + sdata->cdata->last_injection_timestamp = 0; + sdata->cdata->injection_odr = 0; + + /* Set start injection */ + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_TEST_REG_ADDR, + ST_ISM330DLC_START_INJECT_XL_MASK, 1, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + sdata->cdata->injection_mode = true; + } + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static ssize_t st_ism330dlc_sysfs_get_injection_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->injection_mode); +} + +static ssize_t st_ism330dlc_sysfs_upload_xl_data(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i, n = 1; + s64 timestamp, deltatime; + u8 sample[3], current_odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (!sdata->cdata->injection_mode) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + for (i = 0; i < 3; i++) + sample[i] = *(s16 *)(&buf[i * 2]) >> 8; + + timestamp = *(s64 *)(buf + ALIGN(6, sizeof(s64))); + + if (sdata->cdata->last_injection_timestamp > 0) { + deltatime = timestamp - sdata->cdata->last_injection_timestamp; + if ((deltatime > ST_ISM330DLC_208HZ_INJECT_NS_DOWN) && + (deltatime <= ST_ISM330DLC_208HZ_INJECT_NS_UP)) { + current_odr = 208; + n = 4; + } else if ((deltatime > ST_ISM330DLC_104HZ_INJECT_NS_DOWN) && + (deltatime <= ST_ISM330DLC_104HZ_INJECT_NS_UP)) { + current_odr = 104; + n = 3; + } else if ((deltatime > ST_ISM330DLC_52HZ_INJECT_NS_DOWN) && + (deltatime <= ST_ISM330DLC_52HZ_INJECT_NS_UP)) { + current_odr = 52; + n = 2; + } else if ((deltatime > ST_ISM330DLC_26HZ_INJECT_NS_DOWN) && + (deltatime <= ST_ISM330DLC_26HZ_INJECT_NS_UP)) { + current_odr = 26; + n = 1; + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + if (sdata->cdata->injection_odr != current_odr) { + err = st_ism330dlc_write_data_with_mask(sdata->cdata, + st_ism330dlc_odr_table.addr[sdata->sindex], + st_ism330dlc_odr_table.mask[sdata->sindex], + st_ism330dlc_odr_table.odr_avl[n].value, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + sdata->cdata->injection_odr = current_odr; + } + } + + sdata->cdata->last_injection_timestamp = timestamp; + + err = sdata->cdata->tf->write(sdata->cdata, ST_ISM330DLC_INJECT_XL_X_ADDR, + 3, (u8 *)sample, false); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + mutex_unlock(&indio_dev->mlock); + + usleep_range(1000, 2000); + + return size; +} + +static ssize_t st_ism330dlc_sysfs_get_injection_sensors(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", "ism330dlc_accel"); +} +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + +ssize_t st_ism330dlc_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + struct ism330dlc_data *cdata = sdata->cdata; + + return scnprintf(buf, PAGE_SIZE, "%u\n", cdata->module_id); +} + +static ST_ISM330DLC_DEV_ATTR_SAMP_FREQ(); +static ST_ISM330DLC_DEV_ATTR_SAMP_FREQ_AVAIL(); +static ST_ISM330DLC_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); +static ST_ISM330DLC_DEV_ATTR_SCALE_AVAIL(in_anglvel_scale_available); + +static ST_ISM330DLC_HWFIFO_ENABLED(); +static ST_ISM330DLC_HWFIFO_WATERMARK(); +static ST_ISM330DLC_HWFIFO_WATERMARK_MIN(); +static ST_ISM330DLC_HWFIFO_WATERMARK_MAX(); +static ST_ISM330DLC_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_ism330dlc_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_ism330dlc_sysfs_get_selftest_status, + st_ism330dlc_sysfs_start_selftest_status, 0); + +static IIO_DEVICE_ATTR(module_id, 0444, st_ism330dlc_get_module_id, NULL, 0); + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION +static IIO_DEVICE_ATTR(injection_mode, S_IWUSR | S_IRUGO, + st_ism330dlc_sysfs_get_injection_mode, + st_ism330dlc_sysfs_set_injection_mode, 0); + +static IIO_DEVICE_ATTR(in_accel_injection_raw, S_IWUSR, NULL, + st_ism330dlc_sysfs_upload_xl_data, 0); + +static IIO_DEVICE_ATTR(injection_sensors, S_IRUGO, + st_ism330dlc_sysfs_get_injection_sensors, + NULL, 0); +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + +static int st_ism330dlc_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == IIO_CHAN_INFO_SCALE) { + if ((chan->type == IIO_ANGL_VEL) || + (chan->type == IIO_ACCEL)) + return IIO_VAL_INT_PLUS_NANO; + } + + return -EINVAL; +} + +static struct attribute *st_ism330dlc_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + &iio_dev_attr_injection_mode.dev_attr.attr, + &iio_dev_attr_in_accel_injection_raw.dev_attr.attr, +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + NULL, +}; + +static const struct attribute_group st_ism330dlc_accel_attribute_group = { + .attrs = st_ism330dlc_accel_attributes, +}; + +static const struct iio_info st_ism330dlc_accel_info = { + .attrs = &st_ism330dlc_accel_attribute_group, + .read_raw = &st_ism330dlc_read_raw, + .write_raw = &st_ism330dlc_write_raw, + .write_raw_get_fmt = st_ism330dlc_write_raw_get_fmt, +}; + +static struct attribute *st_ism330dlc_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.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_ism330dlc_gyro_attribute_group = { + .attrs = st_ism330dlc_gyro_attributes, +}; + +static const struct iio_info st_ism330dlc_gyro_info = { + .attrs = &st_ism330dlc_gyro_attribute_group, + .read_raw = &st_ism330dlc_read_raw, + .write_raw = &st_ism330dlc_write_raw, + .write_raw_get_fmt = st_ism330dlc_write_raw_get_fmt, +}; + +static struct attribute *st_ism330dlc_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + NULL, +}; + +static const struct attribute_group st_ism330dlc_tilt_attribute_group = { + .attrs = st_ism330dlc_tilt_attributes, +}; + +static const struct iio_info st_ism330dlc_tilt_info = { + .attrs = &st_ism330dlc_tilt_attribute_group, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops st_ism330dlc_trigger_ops = { + .set_trigger_state = ST_ISM330DLC_TRIGGER_SET_STATE, +}; +#define ST_ISM330DLC_TRIGGER_OPS (&st_ism330dlc_trigger_ops) +#else +#define ST_ISM330DLC_TRIGGER_OPS NULL +#endif + +static void st_ism330dlc_get_properties(struct ism330dlc_data *cdata) +{ + if (device_property_read_u32(cdata->dev, "st,module_id", + &cdata->module_id)) { + cdata->module_id = 1; + } +} + +int st_ism330dlc_common_probe(struct ism330dlc_data *cdata, int irq) +{ + u8 wai = 0x00; + int i, n, err; + struct ism330dlc_sensor_data *sdata; + + mutex_init(&cdata->bank_registers_lock); + mutex_init(&cdata->fifo_lock); + mutex_init(&cdata->tb.buf_lock); + mutex_init(&cdata->odr_lock); + + cdata->fifo_watermark = 0; + cdata->fifo_status = BYPASS; + cdata->enable_digfunc_mask = 0; + +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + cdata->enable_sensorhub_mask = 0; +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + cdata->irq_enable_fifo_mask = 0; + cdata->irq_enable_accel_ext_mask = 0; + + for (i = 0; i < ST_INDIO_DEV_NUM + 1; i++) { + cdata->hw_odr[i] = 0; + cdata->v_odr[i] = 0; + cdata->hwfifo_enabled[i] = false; + cdata->hwfifo_decimator[i] = 0; + cdata->hwfifo_watermark[i] = 1; + cdata->nofifo_decimation[i].decimator = 1; + cdata->nofifo_decimation[i].num_samples = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].decimator = 1; + cdata->fifo_output[i].timestamp_p = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].initialized = false; + } + + cdata->sensors_use_fifo = 0; + cdata->sensors_enabled = 0; + + cdata->gyro_selftest_status = 0; + cdata->accel_selftest_status = 0; + + cdata->accel_on = false; + cdata->magn_on = false; + + cdata->accel_odr_dependency[0] = 0; + cdata->accel_odr_dependency[1] = 0; + cdata->accel_odr_dependency[2] = 0; + + cdata->trigger_odr = 0; + + cdata->fifo_data = kmalloc(ST_ISM330DLC_MAX_FIFO_SIZE * + sizeof(u8), GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + cdata->injection_mode = false; + cdata->last_injection_timestamp = 0; + cdata->injection_odr = 0; +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + + err = cdata->tf->read(cdata, ST_ISM330DLC_WAI_ADDRESS, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + goto free_fifo_data; + } + if (wai != ST_ISM330DLC_WAI_EXP) { + dev_err(cdata->dev, + "Who-Am-I value not valid. Expected %x, Found %x\n", + ST_ISM330DLC_WAI_EXP, wai); + err = -ENODEV; + goto free_fifo_data; + } + + st_ism330dlc_get_properties(cdata); + + if (irq > 0) { + cdata->irq = irq; + } else { + err = -EINVAL; + dev_info(cdata->dev, + "DRDY not available, curernt implementation needs irq!\n"); + goto free_fifo_data; + } + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + cdata->indio_dev[i] = devm_iio_device_alloc(cdata->dev, + sizeof(struct ism330dlc_sensor_data)); + if (!cdata->indio_dev[i]) { + err = -ENOMEM; + goto free_fifo_data; + } + + sdata = iio_priv(cdata->indio_dev[i]); + sdata->cdata = cdata; + sdata->sindex = i; + + switch (i) { + case ST_MASK_ID_ACCEL: + sdata->data_out_reg = st_ism330dlc_accel_ch[0].address; + cdata->v_odr[i] = st_ism330dlc_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_ism330dlc_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_GYRO: + sdata->data_out_reg = st_ism330dlc_gyro_ch[0].address; + cdata->v_odr[i] = st_ism330dlc_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_ism330dlc_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = 0; + sdata->num_data_channels = 3; + break; + default: + sdata->num_data_channels = 0; + break; + } + + cdata->indio_dev[i]->modes = INDIO_DIRECT_MODE; + } + + cdata->indio_dev[ST_MASK_ID_ACCEL]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_ISM330DLC_ACCEL_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_ACCEL]->info = &st_ism330dlc_accel_info; + cdata->indio_dev[ST_MASK_ID_ACCEL]->channels = st_ism330dlc_accel_ch; + cdata->indio_dev[ST_MASK_ID_ACCEL]->num_channels = + ARRAY_SIZE(st_ism330dlc_accel_ch); + + cdata->indio_dev[ST_MASK_ID_GYRO]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_ISM330DLC_GYRO_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_GYRO]->info = &st_ism330dlc_gyro_info; + cdata->indio_dev[ST_MASK_ID_GYRO]->channels = st_ism330dlc_gyro_ch; + cdata->indio_dev[ST_MASK_ID_GYRO]->num_channels = + ARRAY_SIZE(st_ism330dlc_gyro_ch); + + cdata->indio_dev[ST_MASK_ID_TILT]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_ISM330DLC_TILT_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_TILT]->info = &st_ism330dlc_tilt_info; + cdata->indio_dev[ST_MASK_ID_TILT]->channels = st_ism330dlc_tilt_ch; + cdata->indio_dev[ST_MASK_ID_TILT]->num_channels = + ARRAY_SIZE(st_ism330dlc_tilt_ch); + + err = st_ism330dlc_init_sensor(cdata); + if (err < 0) + goto free_fifo_data; + + err = st_ism330dlc_allocate_rings(cdata); + if (err < 0) + goto free_fifo_data; + + if (irq > 0) { + err = st_ism330dlc_allocate_triggers(cdata, + ST_ISM330DLC_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_device_register(cdata->indio_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + st_ism330dlc_i2c_master_probe(cdata); + + device_init_wakeup(cdata->dev, true); + + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->indio_dev[n]); + + if (irq > 0) + st_ism330dlc_deallocate_triggers(cdata); +deallocate_ring: + st_ism330dlc_deallocate_rings(cdata); +free_fifo_data: + kfree(cdata->fifo_data); + + return err; +} +EXPORT_SYMBOL(st_ism330dlc_common_probe); + +void st_ism330dlc_common_remove(struct ism330dlc_data *cdata, int irq) +{ + int i; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_device_unregister(cdata->indio_dev[i]); + + if (irq > 0) + st_ism330dlc_deallocate_triggers(cdata); + + st_ism330dlc_deallocate_rings(cdata); + + kfree(cdata->fifo_data); + + st_ism330dlc_i2c_master_exit(cdata); +} +EXPORT_SYMBOL(st_ism330dlc_common_remove); + +#ifdef CONFIG_PM +int __maybe_unused st_ism330dlc_common_suspend(struct ism330dlc_data *cdata) +{ + int err, i; + u8 tmp_sensors_enabled; + struct ism330dlc_sensor_data *sdata; + + tmp_sensors_enabled = cdata->sensors_enabled; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if (i == ST_MASK_ID_TILT) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + err = st_ism330dlc_set_enable(sdata, false, true); + if (err < 0) + return err; + } + cdata->sensors_enabled = tmp_sensors_enabled; + + if (cdata->sensors_enabled & ST_ISM330DLC_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + enable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_ism330dlc_common_suspend); + +int __maybe_unused st_ism330dlc_common_resume(struct ism330dlc_data *cdata) +{ + int err, i; + struct ism330dlc_sensor_data *sdata; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if (i == ST_MASK_ID_TILT) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + + if (BIT(sdata->sindex) & cdata->sensors_enabled) { + err = st_ism330dlc_set_enable(sdata, true, true); + if (err < 0) + return err; + } + } + + if (cdata->sensors_enabled & ST_ISM330DLC_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + disable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_ism330dlc_common_resume); +#endif /* CONFIG_PM */ + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics ism330dlc core driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c.c b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c.c new file mode 100644 index 000000000000..ef4895dcd7e7 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c.c @@ -0,0 +1,182 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism330dlc i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_ism330dlc.h" + +static int st_ism330dlc_i2c_read(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int st_ism330dlc_i2c_write(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + int err = 0; + u8 send[8]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, &msg, 1); + + return err; +} + +static const struct st_ism330dlc_transfer_function st_ism330dlc_tf_i2c = { + .write = st_ism330dlc_i2c_write, + .read = st_ism330dlc_i2c_read, +}; + +static int st_ism330dlc_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct ism330dlc_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + i2c_set_clientdata(client, cdata); + + cdata->tf = &st_ism330dlc_tf_i2c; + + err = st_ism330dlc_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_ism330dlc_i2c_remove(struct i2c_client *client) +{ + struct ism330dlc_data *cdata = i2c_get_clientdata(client); + + st_ism330dlc_common_remove(cdata, client->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_ism330dlc_suspend(struct device *dev) +{ + struct ism330dlc_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_ism330dlc_common_suspend(cdata); +} + +static int __maybe_unused st_ism330dlc_resume(struct device *dev) +{ + struct ism330dlc_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_ism330dlc_common_resume(cdata); +} + +static const struct dev_pm_ops st_ism330dlc_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_ism330dlc_suspend, st_ism330dlc_resume) +}; + +#define ST_ISM330DLC_PM_OPS (&st_ism330dlc_pm_ops) +#else /* CONFIG_PM */ +#define ST_ISM330DLC_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id st_ism330dlc_id_table[] = { + { ISM330DLC_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_ism330dlc_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id ism330dlc_of_match[] = { + { + .compatible = "st,ism330dlc", + .data = ISM330DLC_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, ism330dlc_of_match); +#else /* CONFIG_OF */ +#define ism330dlc_of_match NULL +#endif /* CONFIG_OF */ + +static struct i2c_driver st_ism330dlc_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-ism330dlc-i2c", + .pm = ST_ISM330DLC_PM_OPS, + .of_match_table = of_match_ptr(ism330dlc_of_match), + }, + .probe = st_ism330dlc_i2c_probe, + .remove = st_ism330dlc_i2c_remove, + .id_table = st_ism330dlc_id_table, +}; +module_i2c_driver(st_ism330dlc_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics ism330dlc i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c_master.c b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c_master.c new file mode 100644 index 000000000000..0df7b73d1ccc --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_i2c_master.c @@ -0,0 +1,1775 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism330dlc i2c master driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_ism330dlc.h" + +#define EXT0_INDEX 0 + +#define ST_ISM330DLC_ODR_LIST_NUM 4 +#define ST_ISM330DLC_SENSOR_HUB_OP_TIMEOUT 5 +#define ST_ISM330DLC_SRC_FUNC_ADDR 0x53 +#define ST_ISM330DLC_EN_BIT 0x01 +#define ST_ISM330DLC_DIS_BIT 0x00 +#define ST_ISM330DLC_SLV0_ADDR_ADDR 0x02 +#define ST_ISM330DLC_SLV1_ADDR_ADDR 0x05 +#define ST_ISM330DLC_SLV2_ADDR_ADDR 0x08 +#define ST_ISM330DLC_SLV0_OUT_ADDR 0x2e +#define ST_ISM330DLC_INTER_PULLUP_ADDR 0x1a +#define ST_ISM330DLC_INTER_PULLUP_MASK 0x08 +#define ST_ISM330DLC_FUNC_MAX_RATE_ADDR 0x18 +#define ST_ISM330DLC_FUNC_MAX_RATE_MASK 0x02 +#define ST_ISM330DLC_DATAWRITE_SLV0 0x0e +#define ST_ISM330DLC_SLVX_READ 0x01 + +/* External sensors configuration */ +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct ism330dlc_sensor_data *sdata); + +#define ST_ISM330DLC_EXT0_ADDR 0x1e +#define ST_ISM330DLC_EXT0_ADDR2 0x1c +#define ST_ISM330DLC_EXT0_WAI_ADDR 0x0f +#define ST_ISM330DLC_EXT0_WAI_VALUE 0x3d +#define ST_ISM330DLC_EXT0_RESET_ADDR 0x21 +#define ST_ISM330DLC_EXT0_RESET_MASK 0x04 +#define ST_ISM330DLC_EXT0_FULLSCALE_ADDR 0x21 +#define ST_ISM330DLC_EXT0_FULLSCALE_MASK 0x60 +#define ST_ISM330DLC_EXT0_FULLSCALE_VALUE 0x02 +#define ST_ISM330DLC_EXT0_ODR_ADDR 0x20 +#define ST_ISM330DLC_EXT0_ODR_MASK 0x1c +#define ST_ISM330DLC_EXT0_ODR0_HZ 10 +#define ST_ISM330DLC_EXT0_ODR0_VALUE 0x04 +#define ST_ISM330DLC_EXT0_ODR1_HZ 20 +#define ST_ISM330DLC_EXT0_ODR1_VALUE 0x05 +#define ST_ISM330DLC_EXT0_ODR2_HZ 40 +#define ST_ISM330DLC_EXT0_ODR2_VALUE 0x06 +#define ST_ISM330DLC_EXT0_ODR3_HZ 80 +#define ST_ISM330DLC_EXT0_ODR3_VALUE 0x07 +#define ST_ISM330DLC_EXT0_PW_ADDR 0x22 +#define ST_ISM330DLC_EXT0_PW_MASK 0x03 +#define ST_ISM330DLC_EXT0_PW_OFF 0x02 +#define ST_ISM330DLC_EXT0_PW_ON 0x00 +#define ST_ISM330DLC_EXT0_GAIN_VALUE 438 +#define ST_ISM330DLC_EXT0_OUT_X_L_ADDR 0x28 +#define ST_ISM330DLC_EXT0_OUT_Y_L_ADDR 0x2a +#define ST_ISM330DLC_EXT0_OUT_Z_L_ADDR 0x2c +#define ST_ISM330DLC_EXT0_READ_DATA_LEN 6 +#define ST_ISM330DLC_EXT0_BDU_ADDR 0x24 +#define ST_ISM330DLC_EXT0_BDU_MASK 0x40 +#define ST_ISM330DLC_EXT0_STD 0 +#define ST_ISM330DLC_EXT0_BOOT_FUNCTION (&lis3mdl_initialization) +#define ST_ISM330DLC_SELFTEST_EXT0_MIN 2281 +#define ST_ISM330DLC_SELFTEST_EXT0_MAX 6843 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN_Z 228 +#define ST_ISM330DLC_SELFTEST_EXT0_MAX_Z 2281 +#define ST_ISM330DLC_SELFTEST_ADDR1 0x20 +#define ST_ISM330DLC_SELFTEST_ADDR2 0x21 +#define ST_ISM330DLC_SELFTEST_ADDR3 0x22 +#define ST_ISM330DLC_SELFTEST_ADDR1_VALUE 0x1c +#define ST_ISM330DLC_SELFTEST_ADDR2_VALUE 0x40 +#define ST_ISM330DLC_SELFTEST_ADDR3_VALUE 0x00 +#define ST_ISM330DLC_SELFTEST_ENABLE 0x1d +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct ism330dlc_sensor_data *sdata); + +#define ST_ISM330DLC_EXT0_ADDR 0x0c +#define ST_ISM330DLC_EXT0_ADDR2 0x0d +#define ST_ISM330DLC_EXT0_WAI_ADDR 0x01 +#define ST_ISM330DLC_EXT0_WAI_VALUE 0x05 +#define ST_ISM330DLC_EXT0_RESET_ADDR 0x32 +#define ST_ISM330DLC_EXT0_RESET_MASK 0x01 +#define ST_ISM330DLC_EXT0_FULLSCALE_ADDR 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_MASK 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_VALUE 0x00 +#define ST_ISM330DLC_EXT0_ODR_ADDR 0x31 +#define ST_ISM330DLC_EXT0_ODR_MASK 0x1f +#define ST_ISM330DLC_EXT0_ODR0_HZ 10 +#define ST_ISM330DLC_EXT0_ODR0_VALUE 0x02 +#define ST_ISM330DLC_EXT0_ODR1_HZ 20 +#define ST_ISM330DLC_EXT0_ODR1_VALUE 0x04 +#define ST_ISM330DLC_EXT0_ODR2_HZ 50 +#define ST_ISM330DLC_EXT0_ODR2_VALUE 0x06 +#define ST_ISM330DLC_EXT0_ODR3_HZ 100 +#define ST_ISM330DLC_EXT0_ODR3_VALUE 0x08 +#define ST_ISM330DLC_EXT0_PW_ADDR ST_ISM330DLC_EXT0_ODR_ADDR +#define ST_ISM330DLC_EXT0_PW_MASK ST_ISM330DLC_EXT0_ODR_MASK +#define ST_ISM330DLC_EXT0_PW_OFF 0x00 +#define ST_ISM330DLC_EXT0_PW_ON ST_ISM330DLC_EXT0_ODR0_VALUE +#define ST_ISM330DLC_EXT0_GAIN_VALUE 6000 +#define ST_ISM330DLC_EXT0_OUT_X_L_ADDR 0x11 +#define ST_ISM330DLC_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_ISM330DLC_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_ISM330DLC_EXT0_READ_DATA_LEN 6 +#define ST_ISM330DLC_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_ISM330DLC_EXT0_SENSITIVITY_LEN 3 +#define ST_ISM330DLC_EXT0_STD 0 +#define ST_ISM330DLC_EXT0_BOOT_FUNCTION (&akm09911_initialization) +#define ST_ISM330DLC_EXT0_DATA_STATUS 0x18 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN (-30) +#define ST_ISM330DLC_SELFTEST_EXT0_MAX 30 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN_Z (-400) +#define ST_ISM330DLC_SELFTEST_EXT0_MAX_Z (-50) +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct ism330dlc_sensor_data *sdata); + +#define ST_ISM330DLC_EXT0_ADDR 0x0c +#define ST_ISM330DLC_EXT0_ADDR2 0x0d +#define ST_ISM330DLC_EXT0_WAI_ADDR 0x01 +#define ST_ISM330DLC_EXT0_WAI_VALUE 0x04 +#define ST_ISM330DLC_EXT0_RESET_ADDR 0x32 +#define ST_ISM330DLC_EXT0_RESET_MASK 0x01 +#define ST_ISM330DLC_EXT0_FULLSCALE_ADDR 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_MASK 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_VALUE 0x00 +#define ST_ISM330DLC_EXT0_ODR_ADDR 0x31 +#define ST_ISM330DLC_EXT0_ODR_MASK 0x1f +#define ST_ISM330DLC_EXT0_ODR0_HZ 10 +#define ST_ISM330DLC_EXT0_ODR0_VALUE 0x02 +#define ST_ISM330DLC_EXT0_ODR1_HZ 20 +#define ST_ISM330DLC_EXT0_ODR1_VALUE 0x04 +#define ST_ISM330DLC_EXT0_ODR2_HZ 50 +#define ST_ISM330DLC_EXT0_ODR2_VALUE 0x06 +#define ST_ISM330DLC_EXT0_ODR3_HZ 100 +#define ST_ISM330DLC_EXT0_ODR3_VALUE 0x08 +#define ST_ISM330DLC_EXT0_PW_ADDR ST_ISM330DLC_EXT0_ODR_ADDR +#define ST_ISM330DLC_EXT0_PW_MASK ST_ISM330DLC_EXT0_ODR_MASK +#define ST_ISM330DLC_EXT0_PW_OFF 0x00 +#define ST_ISM330DLC_EXT0_PW_ON ST_ISM330DLC_EXT0_ODR0_VALUE +#define ST_ISM330DLC_EXT0_GAIN_VALUE 1500 +#define ST_ISM330DLC_EXT0_OUT_X_L_ADDR 0x11 +#define ST_ISM330DLC_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_ISM330DLC_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_ISM330DLC_EXT0_READ_DATA_LEN 6 +#define ST_ISM330DLC_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_ISM330DLC_EXT0_SENSITIVITY_LEN 3 +#define ST_ISM330DLC_EXT0_STD 0 +#define ST_ISM330DLC_EXT0_BOOT_FUNCTION (&akm09912_initialization) +#define ST_ISM330DLC_EXT0_DATA_STATUS 0x18 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN (-200) +#define ST_ISM330DLC_SELFTEST_EXT0_MAX 200 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN_Z (-1600) +#define ST_ISM330DLC_SELFTEST_EXT0_MAX_Z (-400) +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09916 +#define ST_ISM330DLC_EXT0_ADDR 0x0c +#define ST_ISM330DLC_EXT0_ADDR2 0x0c +#define ST_ISM330DLC_EXT0_WAI_ADDR 0x01 +#define ST_ISM330DLC_EXT0_WAI_VALUE 0x09 +#define ST_ISM330DLC_EXT0_RESET_ADDR 0x32 +#define ST_ISM330DLC_EXT0_RESET_MASK 0x01 +#define ST_ISM330DLC_EXT0_FULLSCALE_ADDR 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_MASK 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_VALUE 0x00 +#define ST_ISM330DLC_EXT0_ODR_ADDR 0x31 +#define ST_ISM330DLC_EXT0_ODR_MASK 0x1f +#define ST_ISM330DLC_EXT0_ODR0_HZ 10 +#define ST_ISM330DLC_EXT0_ODR0_VALUE 0x02 +#define ST_ISM330DLC_EXT0_ODR1_HZ 20 +#define ST_ISM330DLC_EXT0_ODR1_VALUE 0x04 +#define ST_ISM330DLC_EXT0_ODR2_HZ 50 +#define ST_ISM330DLC_EXT0_ODR2_VALUE 0x06 +#define ST_ISM330DLC_EXT0_ODR3_HZ 100 +#define ST_ISM330DLC_EXT0_ODR3_VALUE 0x08 +#define ST_ISM330DLC_EXT0_PW_ADDR ST_ISM330DLC_EXT0_ODR_ADDR +#define ST_ISM330DLC_EXT0_PW_MASK ST_ISM330DLC_EXT0_ODR_MASK +#define ST_ISM330DLC_EXT0_PW_OFF 0x00 +#define ST_ISM330DLC_EXT0_PW_ON ST_ISM330DLC_EXT0_ODR0_VALUE +#define ST_ISM330DLC_EXT0_GAIN_VALUE 1500 +#define ST_ISM330DLC_EXT0_OUT_X_L_ADDR 0x11 +#define ST_ISM330DLC_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_ISM330DLC_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_ISM330DLC_EXT0_READ_DATA_LEN 6 +#define ST_ISM330DLC_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_ISM330DLC_EXT0_SENSITIVITY_LEN 3 +#define ST_ISM330DLC_EXT0_STD 0 +#define ST_ISM330DLC_EXT0_BOOT_FUNCTION NULL +#define ST_ISM330DLC_EXT0_DATA_STATUS 0x18 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN (-200) +#define ST_ISM330DLC_SELFTEST_EXT0_MAX 200 +#define ST_ISM330DLC_SELFTEST_EXT0_MIN_Z (-1000) +#define ST_ISM330DLC_SELFTEST_EXT0_MAX_Z (-200) +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09916 */ + + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct ism330dlc_sensor_data *sdata); + +#define ST_ISM330DLC_EXT0_ADDR 0x5d +#define ST_ISM330DLC_EXT0_ADDR2 0x5c +#define ST_ISM330DLC_EXT0_WAI_ADDR 0x0f +#define ST_ISM330DLC_EXT0_WAI_VALUE 0xb1 +#define ST_ISM330DLC_EXT0_RESET_ADDR 0x11 +#define ST_ISM330DLC_EXT0_RESET_MASK 0x80 +#define ST_ISM330DLC_EXT0_FULLSCALE_ADDR 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_MASK 0x00 +#define ST_ISM330DLC_EXT0_FULLSCALE_VALUE 0x00 +#define ST_ISM330DLC_EXT0_ODR_ADDR 0x10 +#define ST_ISM330DLC_EXT0_ODR_MASK 0x70 +#define ST_ISM330DLC_EXT0_ODR0_HZ 1 +#define ST_ISM330DLC_EXT0_ODR0_VALUE 0x01 +#define ST_ISM330DLC_EXT0_ODR1_HZ 10 +#define ST_ISM330DLC_EXT0_ODR1_VALUE 0x02 +#define ST_ISM330DLC_EXT0_ODR2_HZ 25 +#define ST_ISM330DLC_EXT0_ODR2_VALUE 0x03 +#define ST_ISM330DLC_EXT0_ODR3_HZ 50 +#define ST_ISM330DLC_EXT0_ODR3_VALUE 0x04 +#define ST_ISM330DLC_EXT0_PW_ADDR ST_ISM330DLC_EXT0_ODR_ADDR +#define ST_ISM330DLC_EXT0_PW_MASK ST_ISM330DLC_EXT0_ODR_MASK +#define ST_ISM330DLC_EXT0_PW_OFF 0x00 +#define ST_ISM330DLC_EXT0_PW_ON ST_ISM330DLC_EXT0_ODR0_VALUE +#define ST_ISM330DLC_EXT0_GAIN_VALUE 244 +#define ST_ISM330DLC_EXT0_OUT_P_L_ADDR 0x28 +#define ST_ISM330DLC_EXT0_OUT_T_L_ADDR 0x2b +#define ST_ISM330DLC_EXT0_READ_DATA_LEN 5 +#define ST_ISM330DLC_EXT0_BDU_ADDR 0x10 +#define ST_ISM330DLC_EXT0_BDU_MASK 0x02 +#define ST_ISM330DLC_EXT0_STD 0 +#define ST_ISM330DLC_EXT0_BOOT_FUNCTION (&lps22hb_initialization) +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS2MDL +static int lis2mdl_initialization(struct ism330dlc_sensor_data *sdata); + +#define ST_ISM330DLC_EXT0_ADDR 0x1e +#define ST_ISM330DLC_EXT0_ADDR2 0x1e +#define ST_ISM330DLC_EXT0_WAI_ADDR 0x4f +#define ST_ISM330DLC_EXT0_WAI_VALUE 0x40 +#define ST_ISM330DLC_EXT0_RESET_ADDR 0x60 +#define ST_ISM330DLC_EXT0_RESET_MASK 0x20 +#define ST_ISM330DLC_EXT0_ODR_ADDR 0x60 +#define ST_ISM330DLC_EXT0_ODR_MASK 0x0c +#define ST_ISM330DLC_EXT0_ODR0_HZ 10 +#define ST_ISM330DLC_EXT0_ODR0_VALUE 0x00 +#define ST_ISM330DLC_EXT0_ODR1_HZ 20 +#define ST_ISM330DLC_EXT0_ODR1_VALUE 0x01 +#define ST_ISM330DLC_EXT0_ODR2_HZ 50 +#define ST_ISM330DLC_EXT0_ODR2_VALUE 0x02 +#define ST_ISM330DLC_EXT0_ODR3_HZ 100 +#define ST_ISM330DLC_EXT0_ODR3_VALUE 0x03 +#define ST_ISM330DLC_EXT0_PW_ADDR 0x60 +#define ST_ISM330DLC_EXT0_PW_MASK 0x03 +#define ST_ISM330DLC_EXT0_PW_OFF 0x02 +#define ST_ISM330DLC_EXT0_PW_ON 0x00 +#define ST_ISM330DLC_EXT0_GAIN_VALUE 1500 +#define ST_ISM330DLC_EXT0_OUT_X_L_ADDR 0x68 +#define ST_ISM330DLC_EXT0_OUT_Y_L_ADDR 0x6a +#define ST_ISM330DLC_EXT0_OUT_Z_L_ADDR 0x6c +#define ST_ISM330DLC_EXT0_READ_DATA_LEN 6 +#define ST_ISM330DLC_EXT0_BDU_ADDR 0x62 +#define ST_ISM330DLC_EXT0_BDU_MASK 0x10 +#define ST_ISM330DLC_EXT0_STD 0 +#define ST_ISM330DLC_EXT0_TEMP_COMP_ADDR 0x60 +#define ST_ISM330DLC_EXT0_TEMP_COMP_MASK 0x80 +#define ST_ISM330DLC_EXT0_OFF_CANC_ADDR 0x61 +#define ST_ISM330DLC_EXT0_OFF_CANC_MASK 0x02 +#define ST_ISM330DLC_EXT0_BOOT_FUNCTION (&lis2mdl_initialization) +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS2MDL */ + +/* SENSORS SUFFIX NAMES */ +#define ST_ISM330DLC_EXT0_SUFFIX_NAME "magn" +#define ST_ISM330DLC_EXT1_SUFFIX_NAME "press" + +#if defined(CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911) +#define ST_ISM330DLC_EXT0_HAS_SELFTEST 1 +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_MAGN */ + +#if defined(CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB) +#define ST_ISM330DLC_EXT0_HAS_FULLSCALE 1 +#endif + +#if defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911) +#define ST_ISM330DLC_EXT0_IS_AKM 1 +#define ST_ISM330DLC_SELFTEST_STATUS_REG 0x10 +#define ST_ISM330DLC_SELFTEST_ADDR 0x31 +#define ST_ISM330DLC_SELFTEST_ENABLE 0x10 +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM0099xx */ + + +struct st_ism330dlc_i2c_master_odr_reg { + unsigned int hz; + u8 value; +}; + +struct st_ism330dlc_i2c_master_odr_table { + u8 addr; + u8 mask; + struct st_ism330dlc_i2c_master_odr_reg odr_avl[ST_ISM330DLC_ODR_LIST_NUM]; +}; + +static int st_ism330dlc_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask); + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB +static const struct iio_chan_spec st_ism330dlc_ext0_ch[] = { + ST_ISM330DLC_LSM_CHANNELS(IIO_PRESSURE, 0, 0, IIO_NO_MOD, IIO_LE, + 24, 24, ST_ISM330DLC_EXT0_OUT_P_L_ADDR, 'u'), + ST_ISM330DLC_LSM_CHANNELS(IIO_TEMP, 0, 1, IIO_NO_MOD, IIO_LE, + 16, 16, ST_ISM330DLC_EXT0_OUT_T_L_ADDR, 's'), + ST_ISM330DLC_FLUSH_CHANNEL(IIO_PRESSURE), + IIO_CHAN_SOFT_TIMESTAMP(2) +}; +#else /* CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB */ +static const struct iio_chan_spec st_ism330dlc_ext0_ch[] = { + ST_ISM330DLC_LSM_CHANNELS(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_ISM330DLC_EXT0_OUT_X_L_ADDR, 's'), + ST_ISM330DLC_LSM_CHANNELS(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_ISM330DLC_EXT0_OUT_Y_L_ADDR, 's'), + ST_ISM330DLC_LSM_CHANNELS(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_ISM330DLC_EXT0_OUT_Z_L_ADDR, 's'), + ST_ISM330DLC_FLUSH_CHANNEL(IIO_MAGN), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB */ + +static int st_ism330dlc_i2c_master_set_odr(struct ism330dlc_sensor_data *sdata, + unsigned int odr, bool force); + +static int st_ism330dlc_i2c_master_write(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock); +static int st_ism330dlc_i2c_master_read(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset); + +#ifdef ST_ISM330DLC_EXT0_HAS_SELFTEST +static ssize_t st_ism330dlc_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_ism330dlc_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_ism330dlc_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +#endif /* ST_ISM330DLC_EXT0_HAS_SELFTEST */ + +static ssize_t st_ism330dlc_i2c_master_sysfs_sampling_frequency_avail( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%d %d %d %d\n", 13, 26, 52, 104); +} + +static ssize_t st_ism330dlc_i2c_master_sysfs_get_sampling_frequency( + struct device *dev, struct device_attribute *attr, char *buf) +{ + struct ism330dlc_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_ism330dlc_i2c_master_sysfs_set_sampling_frequency( + struct device *dev, struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + mutex_lock(&sdata->cdata->odr_lock); + + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_ism330dlc_i2c_master_set_odr(sdata, odr, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_ism330dlc_i2c_master_sysfs_get_sampling_frequency, + st_ism330dlc_i2c_master_sysfs_set_sampling_frequency); + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL( + st_ism330dlc_i2c_master_sysfs_sampling_frequency_avail); + +static ST_ISM330DLC_HWFIFO_ENABLED(); +static ST_ISM330DLC_HWFIFO_WATERMARK(); +static ST_ISM330DLC_HWFIFO_WATERMARK_MIN(); +static ST_ISM330DLC_HWFIFO_WATERMARK_MAX(); +static ST_ISM330DLC_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(module_id, 0444, st_ism330dlc_get_module_id, NULL, 0); + +#ifdef ST_ISM330DLC_EXT0_HAS_SELFTEST +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_ism330dlc_i2c_master_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_ism330dlc_i2c_master_sysfs_get_selftest_status, + st_ism330dlc_i2c_master_sysfs_start_selftest, 0); +#endif /* ST_ISM330DLC_EXT0_HAS_SELFTEST */ + +static struct attribute *st_ism330dlc_ext0_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef ST_ISM330DLC_EXT0_HAS_SELFTEST + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, +#endif /* ST_ISM330DLC_EXT0_HAS_SELFTEST */ + NULL, +}; + +static const struct attribute_group st_ism330dlc_ext0_attribute_group = { + .attrs = st_ism330dlc_ext0_attributes, +}; + +static const struct iio_info st_ism330dlc_ext0_info = { + .attrs = &st_ism330dlc_ext0_attribute_group, + .read_raw = &st_ism330dlc_i2c_master_read_raw, +}; + +struct st_ism330dlc_iio_info_data { + char suffix_name[20]; + struct iio_info *info; + struct iio_chan_spec *channels; + int num_channels; +}; + +struct st_ism330dlc_reg { + u8 addr; + u8 mask; + u8 def_value; +}; + +struct st_ism330dlc_power_reg { + u8 addr; + u8 mask; + u8 off_value; + u8 on_value; + bool isodr; +}; + +struct st_ism330dlc_custom_function { + int (*boot_initialization)(struct ism330dlc_sensor_data *sdata); +}; + +static struct st_ism330dlc_exs_list { + struct st_ism330dlc_reg wai; + struct st_ism330dlc_reg reset; + struct st_ism330dlc_reg fullscale; + struct st_ism330dlc_i2c_master_odr_table odr; + struct st_ism330dlc_power_reg power; + u8 fullscale_value; + u8 samples_to_discard; + u8 read_data_len; + u8 num_data_channels; + bool available; + unsigned int gain; + u8 i2c_addr; + struct st_ism330dlc_iio_info_data data; + struct st_ism330dlc_custom_function cf; +} st_ism330dlc_exs_list[] = { + { + .wai = { + .addr = ST_ISM330DLC_EXT0_WAI_ADDR, + .def_value = ST_ISM330DLC_EXT0_WAI_VALUE, + }, + .reset = { + .addr = ST_ISM330DLC_EXT0_RESET_ADDR, + .mask = ST_ISM330DLC_EXT0_RESET_MASK, + }, +#ifdef ST_ISM330DLC_EXT0_HAS_FULLSCALE + .fullscale = { + .addr = ST_ISM330DLC_EXT0_FULLSCALE_ADDR, + .mask = ST_ISM330DLC_EXT0_FULLSCALE_MASK, + .def_value = ST_ISM330DLC_EXT0_FULLSCALE_VALUE, + }, +#endif + .odr = { + .addr = ST_ISM330DLC_EXT0_ODR_ADDR, + .mask = ST_ISM330DLC_EXT0_ODR_MASK, + .odr_avl = { + { + .hz = ST_ISM330DLC_EXT0_ODR0_HZ, + .value = ST_ISM330DLC_EXT0_ODR0_VALUE, + }, + { + .hz = ST_ISM330DLC_EXT0_ODR1_HZ, + .value = ST_ISM330DLC_EXT0_ODR1_VALUE, + }, + { + .hz = ST_ISM330DLC_EXT0_ODR2_HZ, + .value = ST_ISM330DLC_EXT0_ODR2_VALUE, + }, + { + .hz = ST_ISM330DLC_EXT0_ODR3_HZ, + .value = ST_ISM330DLC_EXT0_ODR3_VALUE, + }, + }, + }, + .power = { + .addr = ST_ISM330DLC_EXT0_PW_ADDR, + .mask = ST_ISM330DLC_EXT0_PW_MASK, + .off_value = ST_ISM330DLC_EXT0_PW_OFF, + .on_value = ST_ISM330DLC_EXT0_PW_ON, + }, + .samples_to_discard = ST_ISM330DLC_EXT0_STD, + .read_data_len = ST_ISM330DLC_EXT0_READ_DATA_LEN, + .num_data_channels = 3, + .available = false, + .gain = ST_ISM330DLC_EXT0_GAIN_VALUE, + .i2c_addr = ST_ISM330DLC_EXT0_ADDR, + .data = { + .suffix_name = ST_ISM330DLC_EXT0_SUFFIX_NAME, + .info = (struct iio_info *)&st_ism330dlc_ext0_info, + .channels = (struct iio_chan_spec *)&st_ism330dlc_ext0_ch, + .num_channels = ARRAY_SIZE(st_ism330dlc_ext0_ch), + }, + .cf.boot_initialization = ST_ISM330DLC_EXT0_BOOT_FUNCTION, + } +}; + +static inline void st_ism330dlc_master_wait_completed(struct ism330dlc_data *cdata) +{ + msleep((1000U / cdata->trigger_odr) + 2); +} + +static int st_ism330dlc_i2c_master_read(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset) +{ + int err; + u8 slave_conf[3]; + + slave_conf[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | + ST_ISM330DLC_SLVX_READ; + slave_conf[1] = reg_addr; + slave_conf[2] = (len & 0x07); + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + err = st_ism330dlc_write_embedded_registers(cdata, + ST_ISM330DLC_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + + if (en_sensor_hub) { + err = st_ism330dlc_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + + st_ism330dlc_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_ISM330DLC_SLV0_OUT_ADDR + + offset, len & 0x07, data, true); + if (err < 0) + goto i2c_master_read_unlock_mutex; + +#ifdef ST_ISM330DLC_EXT0_IS_AKM + if (read_status_end) { + slave_conf[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + slave_conf[1] = ST_ISM330DLC_EXT0_DATA_STATUS; + slave_conf[2] = 0x01; + + err = st_ism330dlc_write_embedded_registers(cdata, + ST_ISM330DLC_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } +#endif /* ST_ISM330DLC_EXT0_IS_AKM */ + + if (en_sensor_hub) { + err = st_ism330dlc_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + +i2c_master_read_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len & 0x07; +} + +static int st_ism330dlc_i2c_master_write(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock) +{ + int err, i = 0; + u8 slave0_conf[2]; + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + while (i < len) { + slave0_conf[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1); + slave0_conf[1] = reg_addr + i; + + err = st_ism330dlc_write_embedded_registers(cdata, + ST_ISM330DLC_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + slave0_conf[0] = data[i]; + + err = st_ism330dlc_write_embedded_registers(cdata, + ST_ISM330DLC_DATAWRITE_SLV0, + slave0_conf, 1); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + if (en_sensor_hub) { + err = st_ism330dlc_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + st_ism330dlc_master_wait_completed(cdata); + + if (en_sensor_hub) { + err = st_ism330dlc_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + i++; + } + + slave0_conf[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_ISM330DLC_EN_BIT; + slave0_conf[1] = st_ism330dlc_exs_list[EXT0_INDEX].wai.addr; + + st_ism330dlc_write_embedded_registers(cdata, + ST_ISM330DLC_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + +i2c_master_write_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len; +} + +static int st_ism330dlc_i2c_master_write_data_with_mask( + struct ism330dlc_data *cdata, u8 reg_addr, u8 mask, u8 data) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + mutex_lock(&cdata->i2c_transfer_lock); + disable_irq(cdata->irq); + + err = st_ism330dlc_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + err = st_ism330dlc_i2c_master_read(cdata, reg_addr, 1, + &old_data, false, false, true, + st_ism330dlc_exs_list[0].read_data_len); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data != old_data) + err = st_ism330dlc_i2c_master_write(cdata, reg_addr, + 1, &new_data, false, false); + + st_ism330dlc_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + + return err; +} + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct ism330dlc_sensor_data *sdata) +{ + + return st_ism330dlc_i2c_master_write_data_with_mask( + sdata->cdata, + ST_ISM330DLC_EXT0_BDU_ADDR, + ST_ISM330DLC_EXT0_BDU_MASK, ST_ISM330DLC_EN_BIT); +} +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct ism330dlc_sensor_data *sdata) +{ + int err; u8 data[ST_ISM330DLC_EXT0_SENSITIVITY_LEN]; + + err = st_ism330dlc_i2c_master_read(sdata->cdata, + ST_ISM330DLC_EXT0_SENSITIVITY_ADDR, + ST_ISM330DLC_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_ism330dlc_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0]) * 1000) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1]) * 1000) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2]) * 1000) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct ism330dlc_sensor_data *sdata) +{ + int err; u8 data[ST_ISM330DLC_EXT0_SENSITIVITY_LEN]; + + err = st_ism330dlc_i2c_master_read(sdata->cdata, + ST_ISM330DLC_EXT0_SENSITIVITY_ADDR, + ST_ISM330DLC_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_ism330dlc_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0] - 128) * 500) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1] - 128) * 500) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2] - 128) * 500) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct ism330dlc_sensor_data *sdata) +{ + + return st_ism330dlc_i2c_master_write_data_with_mask( + sdata->cdata, + ST_ISM330DLC_EXT0_BDU_ADDR, + ST_ISM330DLC_EXT0_BDU_MASK, ST_ISM330DLC_EN_BIT); +} +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LPS22HB */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS2MDL +static int lis2mdl_initialization(struct ism330dlc_sensor_data *sdata) +{ + int err; + + err = st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_EXT0_TEMP_COMP_ADDR, + ST_ISM330DLC_EXT0_TEMP_COMP_MASK, + 1); + if (err < 0) + return err; + + err = st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_EXT0_OFF_CANC_ADDR, + ST_ISM330DLC_EXT0_OFF_CANC_MASK, + 1); + if (err < 0) + return err; + + return st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + ST_ISM330DLC_EXT0_BDU_ADDR, + ST_ISM330DLC_EXT0_BDU_MASK, + ST_ISM330DLC_EN_BIT); +} +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS2MDL */ + +#ifdef ST_ISM330DLC_EXT0_HAS_SELFTEST +static ssize_t st_ism330dlc_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "absolute\n"); +} + +static ssize_t st_ism330dlc_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + result = sdata->cdata->ext0_selftest_status; + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_ISM330DLC_SELFTEST_NA_MS; + else if (result < 0) + message = ST_ISM330DLC_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_ISM330DLC_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_ism330dlc_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + int x_selftest = 0, y_selftest = 0, z_selftest = 0; + u8 outdata[8], reg_addr, reg_status = 0, temp_reg_status; +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL + int i, x = 0, y = 0, z = 0; + u8 reg_status2 = 0, reg_status3 = 0; + u8 reg_addr2, reg_addr3, temp_reg_status2, temp_reg_status3; +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ +#ifdef ST_ISM330DLC_EXT0_IS_AKM + u8 temp, sh_config[3], timeout = 0; +#endif /* ST_ISM330DLC_EXT0_IS_AKM */ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + sdata->cdata->ext0_selftest_status = 0; + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + if (strncmp(buf, "absolute", size - 2) != 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = st_ism330dlc_enable_sensor_hub(sdata->cdata, true, ST_MASK_ID_EXT0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL + reg_addr = ST_ISM330DLC_SELFTEST_ADDR1; + temp_reg_status = ST_ISM330DLC_SELFTEST_ADDR1_VALUE; + reg_addr2 = ST_ISM330DLC_SELFTEST_ADDR2; + temp_reg_status2 = ST_ISM330DLC_SELFTEST_ADDR2_VALUE; + reg_addr3 = ST_ISM330DLC_SELFTEST_ADDR3; + temp_reg_status3 = ST_ISM330DLC_SELFTEST_ADDR3_VALUE; +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ + +#ifdef ST_ISM330DLC_EXT0_IS_AKM + reg_addr = ST_ISM330DLC_SELFTEST_ADDR; + temp_reg_status = ST_ISM330DLC_SELFTEST_ENABLE; +#endif /* ST_ISM330DLC_EXT0_IS_AKM */ + + err = st_ism330dlc_i2c_master_read(sdata->cdata, reg_addr, 1, + ®_status, false, true, false, + st_ism330dlc_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + +#ifdef ST_ISM330DLC_EXT0_IS_AKM + /* SLAVE 1 is disabled for a while, dummy write to wai reg */ + sh_config[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_ism330dlc_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_ism330dlc_write_embedded_registers(sdata->cdata, + ST_ISM330DLC_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; + + /* SLAVE 2 is disabled for a while, dummy read of wai reg */ + sh_config[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_ism330dlc_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_ism330dlc_write_embedded_registers(sdata->cdata, + ST_ISM330DLC_SLV2_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; +#endif /* ST_ISM330DLC_EXT0_IS_AKM */ + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL + err = st_ism330dlc_i2c_master_read(sdata->cdata, reg_addr2, 1, + ®_status2, false, true, false, + st_ism330dlc_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + + err = st_ism330dlc_i2c_master_read(sdata->cdata, reg_addr3, 1, + ®_status3, false, true, false, + st_ism330dlc_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto disable_sensor_hub; + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr2, 1, + &temp_reg_status2, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr3, 1, + &temp_reg_status3, false, true); + if (err < 0) + goto restore_status_reg2; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_ism330dlc_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 10; + y += ((s16)*(u16 *)&outdata[2]) / 10; + z += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + temp_reg_status = ST_ISM330DLC_SELFTEST_ENABLE; + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto restore_status_reg3; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_ism330dlc_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 10; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 10; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); + if (err < 0) + goto restore_status_reg3; + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); + if (err < 0) + goto restore_status_reg2; + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_ism330dlc_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + if ((abs(x_selftest - x) < ST_ISM330DLC_SELFTEST_EXT0_MIN) || + (abs(x_selftest - x) > ST_ISM330DLC_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(y_selftest - y) < ST_ISM330DLC_SELFTEST_EXT0_MIN) || + (abs(y_selftest - y) > ST_ISM330DLC_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(z_selftest - z) < ST_ISM330DLC_SELFTEST_EXT0_MIN_Z) || + (abs(z_selftest - z) > ST_ISM330DLC_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ + +#ifdef ST_ISM330DLC_EXT0_IS_AKM + do { + msleep(1000U / sdata->cdata->trigger_odr); + + err = st_ism330dlc_i2c_master_read(sdata->cdata, + ST_ISM330DLC_SELFTEST_STATUS_REG, 1, + &temp, false, true, false, 1); + if (err < 0) + goto restore_status_reg; + + timeout++; + } while (((temp & 0x01) == 0) && (timeout < 5)); + + if (timeout >= 5) { + err = -EINVAL; + goto restore_status_reg; + } + + err = st_ism330dlc_i2c_master_read(sdata->cdata, + st_ism330dlc_exs_list[0].data.channels[0].address, + st_ism330dlc_exs_list[0].read_data_len, + outdata, false, true, true, 1); + if (err < 0) + goto restore_status_reg; + +#ifdef ST_ISM330DLC_EXT0_IS_AKM + /* SLAVE 2 recovering */ + sh_config[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_ism330dlc_exs_list[0].data.channels[0].address; + sh_config[2] = st_ism330dlc_exs_list[0].read_data_len; + + err = st_ism330dlc_write_embedded_registers(sdata->cdata, + ST_ISM330DLC_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto restore_status_reg; +#endif /* ST_ISM330DLC_EXT0_IS_AKM */ + + err = st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_ism330dlc_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + x_selftest = ((s16)*(u16 *)&outdata[0]); + y_selftest = ((s16)*(u16 *)&outdata[2]); + z_selftest = ((s16)*(u16 *)&outdata[4]); + +#if defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_ISM330DLC_IIO_EXT0_AKM09911) + + x_selftest *= sdata->c_gain[0]; + y_selftest *= sdata->c_gain[1]; + z_selftest *= sdata->c_gain[2]; + + x_selftest /= 10000; + y_selftest /= 10000; + z_selftest /= 10000; +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_AKM0991X */ + + if ((x_selftest < ST_ISM330DLC_SELFTEST_EXT0_MIN) || + (x_selftest > ST_ISM330DLC_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((y_selftest < ST_ISM330DLC_SELFTEST_EXT0_MIN) || + (y_selftest > ST_ISM330DLC_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((z_selftest < ST_ISM330DLC_SELFTEST_EXT0_MIN_Z) || + (z_selftest > ST_ISM330DLC_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* ST_ISM330DLC_EXT0_IS_AKM */ + + sdata->cdata->ext0_selftest_status = 1; + + mutex_unlock(&sdata->cdata->odr_lock); + + return size; + +#ifdef CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL +restore_status_reg3: + st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); +restore_status_reg2: + st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); +#endif /* CONFIG_ST_ISM330DLC_IIO_EXT0_LIS3MDL */ +restore_status_reg: + st_ism330dlc_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); +disable_sensor_hub: + st_ism330dlc_enable_sensor_hub(sdata->cdata, false, ST_MASK_ID_EXT0); + mutex_unlock(&sdata->cdata->odr_lock); + return err; +} +#endif /* ST_ISM330DLC_EXT0_HAS_SELFTEST */ + + +static int st_ism330dlc_i2c_master_set_odr(struct ism330dlc_sensor_data *sdata, + unsigned int odr, bool force) +{ + int i, err, err2; + u8 value, mask, addr; + bool scan_odr = true; + unsigned int current_odr = sdata->cdata->v_odr[sdata->sindex]; + unsigned int current_hw_odr = sdata->cdata->hw_odr[sdata->sindex]; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + if (scan_odr) { + switch (odr) { + case 13: + case 26: + case 52: + case 104: + break; + default: + return -EINVAL; + } + + for (i = 0; i < ST_ISM330DLC_ODR_LIST_NUM; i++) { + if (st_ism330dlc_exs_list[0].odr.odr_avl[i].hz >= odr) + break; + } + if (i == ST_ISM330DLC_ODR_LIST_NUM) + i--; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = odr; + return 0; + } + } + + addr = st_ism330dlc_exs_list[0].odr.addr; + mask = st_ism330dlc_exs_list[0].odr.mask; + value = st_ism330dlc_exs_list[0].odr.odr_avl[i].value; + } else { + if (st_ism330dlc_exs_list[0].power.isodr) { + addr = st_ism330dlc_exs_list[0].power.addr; + mask = st_ism330dlc_exs_list[0].power.mask; + value = st_ism330dlc_exs_list[0].power.off_value; + } else + goto skip_i2c_write; + } + + sdata->cdata->samples_to_discard[ST_MASK_ID_EXT0] = + st_ism330dlc_exs_list[0].samples_to_discard; + + err = st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + addr, mask, value); + if (err < 0) + return err; + +skip_i2c_write: + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = odr; + + if (!force) { + sdata->cdata->v_odr[sdata->sindex] = odr; + + err = st_ism330dlc_enable_sensor_hub(sdata->cdata, + true, ST_MASK_ID_EXT0); + if (err < 0) { + sdata->cdata->hw_odr[sdata->sindex] = current_hw_odr; + sdata->cdata->v_odr[sdata->sindex] = current_odr; + do { + err2 = st_ism330dlc_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + msleep(200); + } while (err2 < 0); + + return err; + } + } + + return 0; +} + +static int st_ism330dlc_i2c_master_set_enable( + struct ism330dlc_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + /* If odr != power this part should enable/disable sensor */ + if (!st_ism330dlc_exs_list[0].power.isodr) { + if (enable) + reg_value = st_ism330dlc_exs_list[0].power.on_value; + else + reg_value = st_ism330dlc_exs_list[0].power.off_value; + + err = st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + st_ism330dlc_exs_list[0].power.addr, + st_ism330dlc_exs_list[0].power.mask, + reg_value); + if (err < 0) + return err; + } + + err = st_ism330dlc_enable_sensor_hub(sdata->cdata, + enable, ST_MASK_ID_EXT0); + if (err < 0) + return err; + + err = st_ism330dlc_i2c_master_set_odr(sdata, + enable ? sdata->cdata->v_odr[sdata->sindex] : 0, true); + if (err < 0) + goto disable_sensorhub; + + if (buffer) { + err = st_ism330dlc_set_drdy_irq(sdata, enable); + if (err < 0) + goto restore_odr; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; + +restore_odr: + st_ism330dlc_i2c_master_set_odr(sdata, + enable ? 0 : sdata->cdata->v_odr[sdata->sindex], true); +disable_sensorhub: + st_ism330dlc_enable_sensor_hub(sdata->cdata, !enable, ST_MASK_ID_EXT0); + + return err; +} + +static int st_ism330dlc_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask) +{ + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + int err, ch_num_byte = ch->scan_type.storagebits >> 3; + u8 outdata[4]; + + if (ch_num_byte > ARRAY_SIZE(outdata)) + return -ENOMEM; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_ism330dlc_i2c_master_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + st_ism330dlc_master_wait_completed(sdata->cdata); + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + ch_num_byte, outdata, true); + if (err < 0) { + st_ism330dlc_i2c_master_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + err = st_ism330dlc_i2c_master_set_enable(sdata, false, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (ch_num_byte > 2) + *val = (s32)get_unaligned_le32(outdata); + else + *val = (s16)get_unaligned_le16(outdata); + + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[ch->scan_index]; + + if (ch->type == IIO_TEMP) { + *val = 1; + *val2 = 0; + return IIO_VAL_INT; + } + + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_ism330dlc_i2c_master_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_ISM330DLC_XL_DATA_INJECTION + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) + return -EBUSY; +#endif /* CONFIG_ST_ISM330DLC_XL_DATA_INJECTION */ + + return 0; +} + +static int st_ism330dlc_i2c_master_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) && + (indio_dev->buffer->length < 2 * ST_ISM330DLC_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_ism330dlc_i2c_master_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + return 0; +} + +static int st_ism330dlc_i2c_master_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct ism330dlc_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_ism330dlc_i2c_master_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : 0; +} + +static const struct iio_trigger_ops st_ism330dlc_i2c_master_trigger_ops = { + .set_trigger_state = &st_ism330dlc_trig_set_state, +}; + +int st_ism330dlc_i2c_master_allocate_trigger(struct ism330dlc_data *cdata) +{ + int err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->trig[ST_MASK_ID_EXT0]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + return -ENOMEM; + } + + iio_trigger_set_drvdata(cdata->trig[ST_MASK_ID_EXT0], + cdata->indio_dev[ST_MASK_ID_EXT0]); + cdata->trig[ST_MASK_ID_EXT0]->ops = &st_ism330dlc_i2c_master_trigger_ops; + cdata->trig[ST_MASK_ID_EXT0]->dev.parent = cdata->dev; + + err = iio_trigger_register(cdata->trig[ST_MASK_ID_EXT0]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + goto deallocate_trigger; + } + + cdata->indio_dev[ST_MASK_ID_EXT0]->trig = cdata->trig[ST_MASK_ID_EXT0]; + + return 0; + +deallocate_trigger: + iio_trigger_free(cdata->trig[ST_MASK_ID_EXT0]); + return err; +} + +static void st_ism330dlc_i2c_master_deallocate_trigger(struct ism330dlc_data *cdata) +{ + iio_trigger_unregister(cdata->trig[ST_MASK_ID_EXT0]); +} + +static const struct iio_buffer_setup_ops st_ism330dlc_i2c_master_buffer_setup_ops = { + .preenable = &st_ism330dlc_i2c_master_buffer_preenable, + .postenable = &st_ism330dlc_i2c_master_buffer_postenable, + .postdisable = &st_ism330dlc_i2c_master_buffer_postdisable, +}; + +static inline irqreturn_t st_ism330dlc_i2c_master_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +static int st_ism330dlc_i2c_master_allocate_buffer(struct ism330dlc_data *cdata) +{ + return iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_EXT0], + &st_ism330dlc_i2c_master_handler_empty, NULL, + &st_ism330dlc_i2c_master_buffer_setup_ops); +} + +static void st_ism330dlc_i2c_master_deallocate_buffer(struct ism330dlc_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_EXT0]); +} + +static int st_ism330dlc_i2c_master_send_sensor_hub_parameters( + struct ism330dlc_sensor_data *sdata) +{ + int err; + u8 sh_config[3]; + + /* SLAVE 0 is used by write */ + sh_config[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_ISM330DLC_EN_BIT; + sh_config[1] = st_ism330dlc_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x20; + + err = st_ism330dlc_write_embedded_registers(sdata->cdata, + ST_ISM330DLC_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + /* SLAVE 1 is used to read output data */ + sh_config[0] = (st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_ISM330DLC_EN_BIT; + sh_config[1] = st_ism330dlc_exs_list[0].data.channels[0].address; + sh_config[2] = st_ism330dlc_exs_list[0].read_data_len; + + err = st_ism330dlc_write_embedded_registers(sdata->cdata, + ST_ISM330DLC_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + return 0; +} + +static int st_ism330dlc_i2c_master_init_sensor(struct ism330dlc_sensor_data *sdata) +{ + int err, ext_num = 0; + + err = st_ism330dlc_i2c_master_send_sensor_hub_parameters(sdata); + if (err < 0) + return err; + + sdata->c_gain[0] = st_ism330dlc_exs_list[ext_num].gain; + sdata->c_gain[1] = st_ism330dlc_exs_list[ext_num].gain; + sdata->c_gain[2] = st_ism330dlc_exs_list[ext_num].gain; + + if ((st_ism330dlc_exs_list[ext_num].power.addr == + st_ism330dlc_exs_list[ext_num].odr.addr) && + (st_ism330dlc_exs_list[ext_num].power.mask == + st_ism330dlc_exs_list[ext_num].odr.mask)) + st_ism330dlc_exs_list[ext_num].power.isodr = true; + else + st_ism330dlc_exs_list[ext_num].power.isodr = false; + + err = st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + st_ism330dlc_exs_list[ext_num].reset.addr, + st_ism330dlc_exs_list[ext_num].reset.mask, + ST_ISM330DLC_EN_BIT); + if (err < 0) + return err; + + usleep_range(200, 1000); + + if (st_ism330dlc_exs_list[ext_num].fullscale.addr > 0) { + err = st_ism330dlc_i2c_master_write_data_with_mask(sdata->cdata, + st_ism330dlc_exs_list[ext_num].fullscale.addr, + st_ism330dlc_exs_list[ext_num].fullscale.mask, + st_ism330dlc_exs_list[ext_num].fullscale.def_value); + if (err < 0) + return err; + } + + if (st_ism330dlc_exs_list[0].cf.boot_initialization != NULL) { + err = st_ism330dlc_exs_list[0].cf.boot_initialization(sdata); + if (err < 0) + return err; + } + + err = st_ism330dlc_i2c_master_set_enable(sdata, false, false); + if (err < 0) + return err; + + return 0; +} + +static int st_ism330dlc_i2c_master_allocate_device(struct ism330dlc_data *cdata) +{ + int err; + struct ism330dlc_sensor_data *sdata_ext; + + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + + sdata_ext->num_data_channels = + st_ism330dlc_exs_list[0].num_data_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->name = kasprintf(GFP_KERNEL, + "%s_%s", cdata->name, + st_ism330dlc_exs_list[0].data.suffix_name); + + cdata->indio_dev[ST_MASK_ID_EXT0]->info = + st_ism330dlc_exs_list[0].data.info; + cdata->indio_dev[ST_MASK_ID_EXT0]->channels = + st_ism330dlc_exs_list[0].data.channels; + cdata->indio_dev[ST_MASK_ID_EXT0]->num_channels = + st_ism330dlc_exs_list[0].data.num_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->modes = INDIO_DIRECT_MODE; + + sdata_ext->data_out_reg = ST_ISM330DLC_SLV0_OUT_ADDR; + + err = st_ism330dlc_i2c_master_init_sensor(sdata_ext); + if (err < 0) + return err; + + err = st_ism330dlc_i2c_master_allocate_buffer(cdata); + if (err < 0) + return err; + + err = st_ism330dlc_i2c_master_allocate_trigger(cdata); + if (err < 0) + goto iio_deallocate_buffer; + + err = iio_device_register(cdata->indio_dev[ST_MASK_ID_EXT0]); + if (err < 0) + goto iio_deallocate_trigger; + + return 0; + +iio_deallocate_trigger: + st_ism330dlc_i2c_master_deallocate_trigger(cdata); +iio_deallocate_buffer: + st_ism330dlc_i2c_master_deallocate_buffer(cdata); + return err; +} + +static void st_ism330dlc_i2c_master_deallocate_device(struct ism330dlc_data *cdata) +{ + iio_device_unregister(cdata->indio_dev[ST_MASK_ID_EXT0]); + st_ism330dlc_i2c_master_deallocate_trigger(cdata); + st_ism330dlc_i2c_master_deallocate_buffer(cdata); +} + +int st_ism330dlc_i2c_master_probe(struct ism330dlc_data *cdata) +{ + int err, i; + u8 sh_config[3]; + u8 wai, i2c_address; + struct ism330dlc_sensor_data *sdata_ext; + + mutex_init(&cdata->i2c_transfer_lock); + cdata->v_odr[ST_MASK_ID_EXT0] = 13; + cdata->ext0_available = false; + cdata->ext0_selftest_status = false; + +#ifdef CONFIG_ST_ISM330DLC_ENABLE_INTERNAL_PULLUP + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_INTER_PULLUP_ADDR, + ST_ISM330DLC_INTER_PULLUP_MASK, + ST_ISM330DLC_EN_BIT, true); + if (err < 0) + return err; +#endif /* CONFIG_ST_ISM330DLC_ENABLE_INTERNAL_PULLUP */ + + err = st_ism330dlc_write_data_with_mask(cdata, + ST_ISM330DLC_FUNC_MAX_RATE_ADDR, + ST_ISM330DLC_FUNC_MAX_RATE_MASK, 1, true); + if (err < 0) + return err; + + cdata->indio_dev[ST_MASK_ID_EXT0] = devm_iio_device_alloc(cdata->dev, + sizeof(*sdata_ext)); + if (!cdata->indio_dev[ST_MASK_ID_EXT0]) + return -ENOMEM; + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + sdata_ext->cdata = cdata; + sdata_ext->sindex = ST_MASK_ID_EXT0; + cdata->samples_to_discard_2[ST_MASK_ID_EXT0] = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].sip = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = 0; + + for (i = 0; i < 2; i++) { + if (i == 0) + i2c_address = ST_ISM330DLC_EXT0_ADDR; + else + i2c_address = ST_ISM330DLC_EXT0_ADDR2; + + /* to check if sensor is available use SLAVE0 first time */ + sh_config[0] = (i2c_address << 1) | 0x01; + sh_config[1] = st_ism330dlc_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x01; + + err = st_ism330dlc_write_embedded_registers(cdata, + ST_ISM330DLC_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + err = st_ism330dlc_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + msleep(100); + + st_ism330dlc_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_ISM330DLC_SLV0_OUT_ADDR, + 1, &wai, true); + if (err < 0) { + err = st_ism330dlc_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + continue; + } + + err = st_ism330dlc_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + st_ism330dlc_exs_list[EXT0_INDEX].i2c_addr = i2c_address; + break; + } + if (i == 2) + goto ext0_sensor_not_available; + + /* after wai check SLAVE0 is used for write, SLAVE1 for async read + and SLAVE2 to read sensor output data */ + + if (wai != st_ism330dlc_exs_list[EXT0_INDEX].wai.def_value) { + dev_err(cdata->dev, "wai value of external sensor 0 mismatch\n"); + return err; + } + + err = st_ism330dlc_i2c_master_allocate_device(cdata); + if (err < 0) + return err; + + cdata->ext0_available = true; + + return 0; + +ext0_sensor_not_available: + dev_err(cdata->dev, "external sensor 0 not available\n"); + + return err; +} +EXPORT_SYMBOL(st_ism330dlc_i2c_master_probe); + +int st_ism330dlc_i2c_master_exit(struct ism330dlc_data *cdata) +{ + if (cdata->ext0_available) + st_ism330dlc_i2c_master_deallocate_device(cdata); + + return 0; +} +EXPORT_SYMBOL(st_ism330dlc_i2c_master_exit); diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_spi.c b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_spi.c new file mode 100644 index 000000000000..91a3176a35bd --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_spi.c @@ -0,0 +1,194 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism330dlc spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_ism330dlc.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_ism330dlc_spi_read(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int st_ism330dlc_spi_write(struct ism330dlc_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_ISM330DLC_TX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static const struct st_ism330dlc_transfer_function st_ism330dlc_tf_spi = { + .write = st_ism330dlc_spi_write, + .read = st_ism330dlc_spi_read, +}; + +static int st_ism330dlc_spi_probe(struct spi_device *spi) +{ + int err; + struct ism330dlc_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + spi_set_drvdata(spi, cdata); + + cdata->tf = &st_ism330dlc_tf_spi; + + err = st_ism330dlc_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_ism330dlc_spi_remove(struct spi_device *spi) +{ + struct ism330dlc_data *cdata = spi_get_drvdata(spi); + + st_ism330dlc_common_remove(cdata, spi->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_ism330dlc_suspend(struct device *dev) +{ + struct ism330dlc_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_ism330dlc_common_suspend(cdata); +} + +static int __maybe_unused st_ism330dlc_resume(struct device *dev) +{ + struct ism330dlc_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_ism330dlc_common_resume(cdata); +} + +static const struct dev_pm_ops st_ism330dlc_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_ism330dlc_suspend, st_ism330dlc_resume) +}; + +#define ST_ISM330DLC_PM_OPS (&st_ism330dlc_pm_ops) +#else /* CONFIG_PM */ +#define ST_ISM330DLC_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id st_ism330dlc_id_table[] = { + { ISM330DLC_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_ism330dlc_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id ism330dlc_of_match[] = { + { + .compatible = "st,ism330dlc", + .data = ISM330DLC_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, ism330dlc_of_match); +#else /* CONFIG_OF */ +#define ism330dlc_of_match NULL +#endif /* CONFIG_OF */ + +static struct spi_driver st_ism330dlc_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-ism330dlc-spi", + .pm = ST_ISM330DLC_PM_OPS, + .of_match_table = of_match_ptr(ism330dlc_of_match), + }, + .probe = st_ism330dlc_spi_probe, + .remove = st_ism330dlc_spi_remove, + .id_table = st_ism330dlc_id_table, +}; +module_spi_driver(st_ism330dlc_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics ism330dlc spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_trigger.c b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_trigger.c new file mode 100644 index 000000000000..5dd622d25883 --- /dev/null +++ b/drivers/iio/stm/imu/st_ism330dlc/st_ism330dlc_trigger.c @@ -0,0 +1,180 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics ism330dlc trigger driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_ism330dlc.h" + +#define ST_ISM330DLC_DIS_BIT 0x00 +#define ST_ISM330DLC_SRC_FUNC_ADDR 0x53 +#define ST_ISM330DLC_FIFO_DATA_AVL_ADDR 0x3b +#define ST_ISM330DLC_ACCEL_DATA_AVL_ADDR 0x1e +#define ST_ISM330DLC_ACCEL_DATA_AVL 0x01 +#define ST_ISM330DLC_GYRO_DATA_AVL 0x02 +#define ST_ISM330DLC_SRC_TILT_DATA_AVL 0x20 +#define ST_ISM330DLC_FIFO_DATA_AVL 0x80 +#define ST_ISM330DLC_FIFO_DATA_OVR 0x40 + + +static irqreturn_t ism330dlc_irq_management(int irq, void *private) +{ + int err; + bool push; + bool force_read_accel = false; + struct ism330dlc_data *cdata = private; + u8 src_accel_gyro = 0, src_dig_func = 0; + + cdata->timestamp = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + (BIT(ST_MASK_ID_ACCEL) | BIT(ST_MASK_ID_GYRO) | + BIT(ST_MASK_ID_EXT0))) { + err = cdata->tf->read(cdata, ST_ISM330DLC_ACCEL_DATA_AVL_ADDR, + 1, &src_accel_gyro, true); + if (err < 0) + goto read_fifo_status; + + if (src_accel_gyro & ST_ISM330DLC_ACCEL_DATA_AVL) { +#ifdef CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + BIT(ST_MASK_ID_EXT0)) { + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples++; + force_read_accel = true; + + if ((cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = 0; + } else { + push = false; + } + + ism330dlc_read_output_data(cdata, ST_MASK_ID_EXT0, push); + } +#endif /* CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT */ + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + BIT(ST_MASK_ID_ACCEL)) { + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples++; + + if ((cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = 0; + } else { + push = false; + } + + ism330dlc_read_output_data(cdata, ST_MASK_ID_ACCEL, push); + } else { + if (force_read_accel) + ism330dlc_read_output_data(cdata, ST_MASK_ID_ACCEL, false); + } + + } + + if (src_accel_gyro & ST_ISM330DLC_GYRO_DATA_AVL) { + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & BIT(ST_MASK_ID_GYRO)) + ism330dlc_read_output_data(cdata, ST_MASK_ID_GYRO, true); + } + } + +read_fifo_status: + if (cdata->sensors_use_fifo) + st_ism330dlc_read_fifo(cdata, false); + + err = cdata->tf->read(cdata, ST_ISM330DLC_SRC_FUNC_ADDR, + 1, &src_dig_func, true); + if (err < 0) + goto exit_irq; + + if ((src_dig_func & ST_ISM330DLC_SRC_TILT_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_TILT))) { + st_ism330dlc_push_data_with_timestamp(cdata, + ST_MASK_ID_TILT, NULL, cdata->timestamp); + } + +exit_irq: + return IRQ_HANDLED; +} + +int st_ism330dlc_allocate_triggers(struct ism330dlc_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[i] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + if (!cdata->trig[i]) { + dev_err(cdata->dev, + "failed to allocate iio trigger.\n"); + err = -ENOMEM; + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]); + cdata->trig[i]->ops = trigger_ops; + cdata->trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, NULL, ism330dlc_irq_management, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_trigger_register(cdata->trig[n]); + if (err < 0) { + dev_err(cdata->dev, + "failed to register iio trigger.\n"); + goto free_irq; + } + cdata->indio_dev[n]->trig = cdata->trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->trig[i]); + + return err; +} +EXPORT_SYMBOL(st_ism330dlc_allocate_triggers); + +void st_ism330dlc_deallocate_triggers(struct ism330dlc_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_trigger_unregister(cdata->trig[i]); +} +EXPORT_SYMBOL(st_ism330dlc_deallocate_triggers); diff --git a/drivers/iio/stm/imu/st_lsm6ds3/Kconfig b/drivers/iio/stm/imu/st_lsm6ds3/Kconfig new file mode 100644 index 000000000000..2038deeecd0e --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/Kconfig @@ -0,0 +1,95 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# st-lsm6ds3 drivers for STMicroelectronics combo sensor +# + +menuconfig ST_LSM6DS3_IIO + tristate "STMicroelectronics LSM6DS3/LSM6DS33 sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select ST_LSM6DS3_I2C_IIO if (I2C) + select ST_LSM6DS3_SPI_IIO if (SPI) + help + This driver supports LSM6DS3 and LSM6DS33 sensors. + It is a gyroscope/accelerometer combo device. + This driver can be built as a module. The module will be called + st-lsm6ds3. + +if ST_LSM6DS3_IIO + +config ST_LSM6DS3_I2C_IIO + tristate + depends on ST_LSM6DS3_IIO + depends on I2C + +config ST_LSM6DS3_SPI_IIO + tristate + depends on ST_LSM6DS3_IIO + depends on SPI + +config ST_LSM6DS3_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on ST_LSM6DS3_IIO + range 0 8192 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +config ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND + bool "Keep Step counter on during suspend" + depends on ST_LSM6DS3_IIO + default n + help + During suspend step counter is kept on if enabled. Only interrupt + is disabled. + +menuconfig ST_LSM6DS3_IIO_MASTER_SUPPORT + bool "I2C master controller" + depends on I2C && ST_LSM6DS3_IIO + default n + help + Added support for I2C master controller. Supported sensors up + to 4. + +if ST_LSM6DS3_IIO_MASTER_SUPPORT + +config ST_LSM6DS3_ENABLE_INTERNAL_PULLUP + bool "Enabled internals pull-up resistors" + default y + +choice + prompt "External sensor 0" + default ST_LSM6DS3_IIO_EXT0_LIS3MDL + help + Choose the external sensor 0 connected to LSM6DS3. + +config ST_LSM6DS3_IIO_EXT0_LIS3MDL + bool "LIS3MDL" +config ST_LSM6DS3_IIO_EXT0_AKM09911 + bool "AKM09911" +config ST_LSM6DS3_IIO_EXT0_AKM09912 + bool "AKM09912" +config ST_LSM6DS3_IIO_EXT0_AKM09916 + bool "AKM09916" +config ST_LSM6DS3_IIO_EXT0_LPS22HB + bool "LPS22HB" +config ST_LSM6DS3_IIO_EXT0_LIS2MDL + bool "LIS2MDL" +endchoice + +endif + +config ST_LSM6DS3_XL_DATA_INJECTION + bool "Enable XL data injection support" + depends on ST_LSM6DS3_IIO + default n + help + This option enables the accelerometer data injection + support. The device functions may so use an injected + pattern instead of taking the real sensor data. + +endif #ST_LSM6DS3_IIO diff --git a/drivers/iio/stm/imu/st_lsm6ds3/Makefile b/drivers/iio/stm/imu/st_lsm6ds3/Makefile new file mode 100644 index 000000000000..92977de9acab --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/Makefile @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Makefile for STMicroelectronics LSM6DS3 sensor. +# + +obj-$(CONFIG_ST_LSM6DS3_IIO) += st_lsm6ds3.o +st_lsm6ds3-objs := st_lsm6ds3_core.o +obj-$(CONFIG_ST_LSM6DS3_I2C_IIO) += st_lsm6ds3_i2c.o +obj-$(CONFIG_ST_LSM6DS3_SPI_IIO) += st_lsm6ds3_spi.o + +st_lsm6ds3-$(CONFIG_IIO_BUFFER) += st_lsm6ds3_buffer.o +st_lsm6ds3-$(CONFIG_IIO_TRIGGER) += st_lsm6ds3_trigger.o +st_lsm6ds3-$(CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT) += st_lsm6ds3_i2c_master.o diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3.h new file mode 100644 index 000000000000..fd8870fa4c8c --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3.h @@ -0,0 +1,376 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lsm6ds3 driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DS3_H +#define ST_LSM6DS3_H + +#include +#include + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +#include +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#define LSM6DS3_DEV_NAME "lsm6ds3" +#define LSM6DS33_DEV_NAME "lsm6ds33" + +enum st_mask_id { + ST_MASK_ID_ACCEL = 0, + ST_MASK_ID_GYRO, + ST_MASK_ID_SIGN_MOTION, + ST_MASK_ID_STEP_COUNTER, + ST_MASK_ID_STEP_DETECTOR, + ST_MASK_ID_TILT, + ST_MASK_ID_EXT0, + ST_MASK_ID_HW_PEDOMETER, + ST_MASK_ID_SENSOR_HUB, + ST_MASK_ID_DIGITAL_FUNC, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP, +}; + +#define ST_INDIO_DEV_NUM 6 + +#define ST_LSM6DS3_TX_MAX_LENGTH 12 +#define ST_LSM6DS3_RX_MAX_LENGTH 8193 + +#define ST_LSM6DS3_BYTE_FOR_CHANNEL 2 +#define ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6 + +#define ST_LSM6DS3_MAX_FIFO_SIZE 8192 +#define ST_LSM6DS3_MAX_FIFO_THRESHOLD 1092 +#define ST_LSM6DS3_MAX_FIFO_LENGHT (ST_LSM6DS3_MAX_FIFO_SIZE / \ + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE) + +#define ST_LSM6DS3_SELFTEST_NA_MS "na" +#define ST_LSM6DS3_SELFTEST_FAIL_MS "fail" +#define ST_LSM6DS3_SELFTEST_PASS_MS "pass" + +#define ST_LSM6DS3_WAKE_UP_SENSORS (BIT(ST_MASK_ID_SIGN_MOTION) | \ + BIT(ST_MASK_ID_TILT)) + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +#define ST_LSM6DS3_NUM_CLIENTS 1 +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ +#define ST_LSM6DS3_NUM_CLIENTS 0 +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#define ST_LSM6DS3_LSM_CHANNELS(device_type, modif, index, mod, \ + endian, sbits, rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +extern const struct iio_event_spec lsm6ds3_fifo_flush_event; + +#define ST_LSM6DS3_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &lsm6ds3_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_LSM6DS3_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + st_lsm6ds3_sysfs_get_hwfifo_enabled,\ + st_lsm6ds3_sysfs_set_hwfifo_enabled, 0); + +#define ST_LSM6DS3_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + st_lsm6ds3_sysfs_get_hwfifo_watermark,\ + st_lsm6ds3_sysfs_set_hwfifo_watermark, 0); + +#define ST_LSM6DS3_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + st_lsm6ds3_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_LSM6DS3_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + st_lsm6ds3_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_LSM6DS3_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + st_lsm6ds3_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +struct st_lsm6ds3_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[ST_LSM6DS3_RX_MAX_LENGTH]; + u8 tx_buf[ST_LSM6DS3_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct lsm6ds3_out_decimation { + short decimator; + short num_samples; +}; + +struct lsm6ds3_fifo_output { + u8 sip; + int64_t deltatime; + int64_t deltatime_default; + int64_t timestamp; + int64_t timestamp_p; + short decimator; + short num_samples; + bool initialized; +}; + +/* struct lsm6ds3_data - common data for i2c or spi driver instance + * @name: pointer to the device name (i2c name or spi modalias). + * @spi_connection: set if driver probed by i2c or spi. + * @enable_digfunc_mask: mask used to enable/disable hw digital functions. + * @enable_pedometer_mask: mask used to enable/disable hw pedometer function. + * @enable_sensorhub_mask: mask used to enable/disable sensor-hub feature. + * @irq_enable_fifo_mask: mask used to enable/disable fifo irq. + * @irq_enable_accel_ext_mask: mask used to enable/disable accel irq. + * @hw_odr: physical sensor odr expressed in Hz. + * @v_odr: requested sensor odr by userspace expressed in Hz. + * @hwfifo_enabled: is hwfifo enabled? + * @hwfifo_decimator: hwfifo decimator factor. + * @hwfifo_watermark: hwfifo watermark value. + * @samples_to_discard: samples to discard due to ODR switch. + * @nofifo_decimation: output status when fifo is disabled. + * @fifo_output: output status when fifo is enabled. + * @sensors_enabled: sensors enabled mask. + * @sensors_use_fifo: sensors use fifo mask. + * @accel_odr_dependency: odr dependency: accel, sensor-hub, dig-func. + * @accel_on: accel is going to be enabled during fifo odr switch? + * @magn_on: magn is going to be enabled during fifo odr switch? + * @odr_lock: mutex to avoid race condition during odr switch. + * @reset_steps: do I need to reset number of steps? + * @sign_motion_event_ready: significan motion event is ready to be pushed. + * @fifo_data: fifo data. + * @gyro_selftest_status: gyroscope selftest result. + * @accel_selftest_status: accelerometer selftest result. + * @irq: irq number. + * @timestamp: timestamp value from boot process. + * @module_id: identify iio devices of the same sensor module. + */ +struct lsm6ds3_data { + const char *name; + + bool spi_connection; + + u16 enable_digfunc_mask; + u16 enable_pedometer_mask; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + u16 enable_sensorhub_mask; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + u16 irq_enable_fifo_mask; + u16 irq_enable_accel_ext_mask; + + unsigned int hw_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int trigger_odr; + + bool hwfifo_enabled[ST_INDIO_DEV_NUM + 1]; + u8 hwfifo_decimator[ST_INDIO_DEV_NUM + 1]; + u16 hwfifo_watermark[ST_INDIO_DEV_NUM + 1]; + u16 fifo_watermark; + + u8 samples_to_discard[ST_INDIO_DEV_NUM + 1]; + u8 samples_to_discard_2[ST_INDIO_DEV_NUM + 1]; + struct lsm6ds3_out_decimation nofifo_decimation[ST_INDIO_DEV_NUM + 1]; + struct lsm6ds3_fifo_output fifo_output[ST_INDIO_DEV_NUM + 1]; + + u16 sensors_enabled; + u16 sensors_use_fifo; + u64 num_steps; + + int accel_odr_dependency[3]; + + bool accel_on; + bool magn_on; + enum fifo_mode fifo_status; + + struct mutex odr_lock; + + bool reset_steps; + bool sign_motion_event_ready; + + u8 *fifo_data; + u8 accel_last_push[6]; + u8 gyro_last_push[6]; + u8 ext0_last_push[6]; + int8_t gyro_selftest_status; + int8_t accel_selftest_status; + + int irq; + + s64 timestamp; + int64_t fifo_enable_timestamp; + int64_t slower_counter; + uint8_t slower_id; + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + bool injection_mode; + s64 last_injection_timestamp; + struct hrtimer injection_timer; + struct work_struct injection_work; + spinlock_t injection_spinlock; + u8 injection_data[30]; + u8 injection_samples; +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + struct work_struct data_work; + + struct device *dev; + struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + 1]; + struct iio_trigger *trig[ST_INDIO_DEV_NUM + 1]; + struct mutex bank_registers_lock; + struct mutex fifo_lock; + u32 module_id; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + bool ext0_available; + int8_t ext0_selftest_status; + struct mutex i2c_transfer_lock; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + const struct st_lsm6ds3_transfer_function *tf; + struct st_lsm6ds3_transfer_buffer tb; +}; + +struct st_lsm6ds3_transfer_function { + int (*write)(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); + int (*read)(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); +}; + +struct lsm6ds3_sensor_data { + struct lsm6ds3_data *cdata; + + unsigned int c_gain[3]; + + u8 num_data_channels; + u8 sindex; + u8 data_out_reg; + u8 *buffer_data; +}; + +int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock); + +int st_lsm6ds3_push_data_with_timestamp(struct lsm6ds3_data *cdata, + u8 index, u8 *data, int64_t timestamp); + +int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq); +void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq); + +int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable, bool buffer); +int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm); +int st_lsm6ds3_enable_sensor_hub(struct lsm6ds3_data *cdata, bool enable, + enum st_mask_id id); +int lsm6ds3_read_output_data(struct lsm6ds3_data *cdata, int sindex, bool push); +int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state); + +ssize_t st_lsm6ds3_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6ds3_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +#ifdef CONFIG_IIO_BUFFER +int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata); +void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata); +int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state); +int st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool async); +#define ST_LSM6DS3_TRIGGER_SET_STATE (&st_lsm6ds3_trig_set_state) +#else /* CONFIG_IIO_BUFFER */ +static inline int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata) +{ + return 0; +} +static inline void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata) +{ +} +static inline int st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool async) +{ + return 0; +} +#define ST_LSM6DS3_TRIGGER_SET_STATE NULL +#endif /* CONFIG_IIO_BUFFER */ + +#ifdef CONFIG_IIO_TRIGGER +int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, + const struct iio_trigger_ops *trigger_ops); +void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata); +void st_lsm6ds3_flush_works(void); +#else /* CONFIG_IIO_TRIGGER */ +static inline int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, + const struct iio_trigger_ops *trigger_ops, int irq) +{ + return 0; +} +static inline void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata, + int irq) +{ + return; +} +static inline void st_lsm6ds3_flush_works(void) +{ + return; +} +#endif /* CONFIG_IIO_TRIGGER */ + +#ifdef CONFIG_PM +int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata); +int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata); +#endif /* CONFIG_PM */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +int st_lsm6ds3_write_embedded_registers(struct lsm6ds3_data *cdata, + u8 reg_addr, u8 *data, int len); +int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata); +int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata); +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ +static inline int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata) +{ + return 0; +} +static inline int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata) +{ + return 0; +} +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#endif /* ST_LSM6DS3_H */ diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_buffer.c new file mode 100644 index 000000000000..5c6d0172e49e --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_buffer.c @@ -0,0 +1,652 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3 buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_lsm6ds3.h" + +#define ST_LSM6DS3_ENABLE_AXIS 0x07 +#define ST_LSM6DS3_FIFO_DIFF_L 0x3a +#define ST_LSM6DS3_FIFO_DIFF_MASK 0x0f +#define ST_LSM6DS3_FIFO_DATA_OUT_L 0x3e +#define ST_LSM6DS3_FIFO_DATA_OVR 0x40 +#define ST_LSM6DS3_FIFO_DATA_EMPTY 0x10 +#define ST_LSM6DS3_STEP_MASK_64BIT (0xFFFFFFFFFFFF0000) + +#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \ + (d) : (c)) : ((b == 0) ? \ + (c) : (d))) + +int st_lsm6ds3_push_data_with_timestamp(struct lsm6ds3_data *cdata, + u8 index, u8 *data, int64_t timestamp) +{ + size_t offset; + int i, n = 0; + struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels; + uint16_t bfch, bfchs_out = 0, bfchs_in = 0; + struct lsm6ds3_sensor_data *sdata = iio_priv(cdata->indio_dev[index]); + + if (timestamp <= cdata->fifo_output[index].timestamp_p) + return -EINVAL; + + for (i = 0; i < sdata->num_data_channels; i++) { + bfch = chs[i].scan_type.storagebits >> 3; + + if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) { + memcpy(&sdata->buffer_data[bfchs_out], + &data[bfchs_in], bfch); + n++; + bfchs_out += bfch; + } + + bfchs_in += bfch; + } + + if (cdata->indio_dev[index]->scan_timestamp) { + offset = cdata->indio_dev[index]->scan_bytes / sizeof(s64) - 1; + ((s64 *)sdata->buffer_data)[offset] = timestamp; + } + + iio_push_to_buffers(cdata->indio_dev[index], sdata->buffer_data); + + cdata->fifo_output[index].timestamp_p = timestamp; + + return 0; +} + +static void st_lsm6ds3_parse_fifo_data(struct lsm6ds3_data *cdata, + u16 read_len, int64_t time_top, u16 num_pattern) +{ + int err; + u16 fifo_offset = 0; + u8 gyro_sip, accel_sip; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + u8 ext0_sip; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + while (fifo_offset < read_len) { + gyro_sip = cdata->fifo_output[ST_MASK_ID_GYRO].sip; + accel_sip = cdata->fifo_output[ST_MASK_ID_ACCEL].sip; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + do { + if (gyro_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_GYRO) + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_GYRO].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp += cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp -= cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + cdata->samples_to_discard[ST_MASK_ID_GYRO] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_GYRO] > 0) + cdata->samples_to_discard[ST_MASK_ID_GYRO]--; + else { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].num_samples >= cdata->fifo_output[ST_MASK_ID_GYRO].decimator) { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_GYRO)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_GYRO] == 0) { + err = st_lsm6ds3_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_GYRO].initialized = true; + + memcpy(cdata->gyro_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_GYRO]--; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].initialized) { + err = st_lsm6ds3_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + cdata->gyro_last_push, + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + gyro_sip--; + } + + if (accel_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_ACCEL) + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp += cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp -= cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + cdata->samples_to_discard[ST_MASK_ID_ACCEL] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_ACCEL] > 0) + cdata->samples_to_discard[ST_MASK_ID_ACCEL]--; + else { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples >= cdata->fifo_output[ST_MASK_ID_ACCEL].decimator) { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] == 0) { + err = st_lsm6ds3_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].initialized = true; + + memcpy(cdata->accel_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_ACCEL]--; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].initialized) { + err = st_lsm6ds3_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + cdata->accel_last_push, + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + accel_sip--; + } + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (ext0_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_EXT0) + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_EXT0].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp += cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp -= cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + cdata->samples_to_discard[ST_MASK_ID_EXT0] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_EXT0] > 0) + cdata->samples_to_discard[ST_MASK_ID_EXT0]--; + else { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].num_samples >= cdata->fifo_output[ST_MASK_ID_EXT0].decimator) { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_EXT0)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_EXT0] == 0) { + err = st_lsm6ds3_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_EXT0].initialized = true; + + memcpy(cdata->ext0_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_EXT0]--; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].initialized) { + err = st_lsm6ds3_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + cdata->ext0_last_push, + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + ext0_sip--; + } + + } while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0)); +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } while ((accel_sip > 0) || (gyro_sip > 0)); +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } +} + +int st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool async) +{ + int err; + u8 fifo_status[2]; +#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read; +#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + u16 read_len = 0, byte_in_pattern, num_pattern; + int64_t temp_counter = 0, timestamp_diff, slower_deltatime; + + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DIFF_L, + 2, fifo_status, true); + if (err < 0) + return err; + + timestamp_diff = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if (fifo_status[1] & ST_LSM6DS3_FIFO_DATA_OVR) { + st_lsm6ds3_set_fifo_mode(cdata, BYPASS); + st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS); + dev_err(cdata->dev, "data fifo overrun, failed to read it.\n"); + return -EINVAL; + } + + if (fifo_status[1] & ST_LSM6DS3_FIFO_DATA_EMPTY) + return 0; + + read_len = ((fifo_status[1] & ST_LSM6DS3_FIFO_DIFF_MASK) << 8) | fifo_status[0]; + read_len *= ST_LSM6DS3_BYTE_FOR_CHANNEL; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip + + cdata->fifo_output[ST_MASK_ID_EXT0].sip) * + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip) * + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + if (byte_in_pattern == 0) + return 0; + + num_pattern = read_len / byte_in_pattern; + + read_len = (read_len / byte_in_pattern) * byte_in_pattern; + if (read_len == 0) + return 0; + +#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO == 0) + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L, + read_len, cdata->fifo_data, true); + if (err < 0) + return err; +#else /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L, + data_to_read, + &cdata->fifo_data[read_len - data_remaining], true); + if (err < 0) + return err; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + + cdata->slower_id = MIN_ID(cdata->fifo_output[ST_MASK_ID_GYRO].sip, + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, + ST_MASK_ID_GYRO, ST_MASK_ID_ACCEL); +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + cdata->slower_id = MIN_ID(cdata->fifo_output[cdata->slower_id].sip, + cdata->fifo_output[ST_MASK_ID_EXT0].sip, + cdata->slower_id, ST_MASK_ID_EXT0); +#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + + temp_counter = cdata->slower_counter; + cdata->slower_counter += (read_len / byte_in_pattern) * cdata->fifo_output[cdata->slower_id].sip; + + if (async) + goto parse_fifo; + + if (temp_counter > 0) { + slower_deltatime = div64_s64(timestamp_diff - cdata->fifo_enable_timestamp, cdata->slower_counter); + + switch (cdata->slower_id) { + case ST_MASK_ID_ACCEL: + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_GYRO: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_EXT0: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = slower_deltatime; + break; + + default: + break; + } + } else { + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default; + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default; +#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + } + +parse_fifo: + st_lsm6ds3_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern); + + return 0; +} + +int lsm6ds3_read_output_data(struct lsm6ds3_data *cdata, int sindex, bool push) +{ + int err; + u8 data[6]; + struct iio_dev *indio_dev = cdata->indio_dev[sindex]; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = cdata->tf->read(cdata, sdata->data_out_reg, + ST_LSM6DS3_BYTE_FOR_CHANNEL * 3, data, true); + if (err < 0) + return err; + + if (push) + st_lsm6ds3_push_data_with_timestamp(cdata, sindex, + data, cdata->timestamp); + + return 0; +} +EXPORT_SYMBOL(lsm6ds3_read_output_data); + +static irqreturn_t st_lsm6ds3_outdata_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static irqreturn_t st_lsm6ds3_step_counter_trigger_handler(int irq, void *p) +{ + int err; + u8 steps_data[2]; + int64_t timestamp = 0; + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if (!sdata->cdata->reset_steps) { + err = sdata->cdata->tf->read(sdata->cdata, + (u8)indio_dev->channels[0].address, + ST_LSM6DS3_BYTE_FOR_CHANNEL, + steps_data, true); + if (err < 0) + goto st_lsm6ds3_step_counter_done; + + sdata->cdata->num_steps = (sdata->cdata->num_steps & + ST_LSM6DS3_STEP_MASK_64BIT) + *((u16 *)steps_data); + timestamp = sdata->cdata->timestamp; + } else { + sdata->cdata->num_steps = 0; + timestamp = + iio_get_time_ns(sdata->cdata->indio_dev[ST_MASK_ID_ACCEL]); + sdata->cdata->reset_steps = false; + } + + memcpy(sdata->buffer_data, (u8 *)&sdata->cdata->num_steps, sizeof(u64)); + + if (indio_dev->scan_timestamp) + *(s64 *)((u8 *)sdata->buffer_data + + ALIGN(ST_LSM6DS3_BYTE_FOR_CHANNEL, + sizeof(s64))) = timestamp; + + iio_push_to_buffers(indio_dev, sdata->buffer_data); + +st_lsm6ds3_step_counter_done: + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static inline irqreturn_t st_lsm6ds3_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state) +{ + return 0; +} + +static int st_lsm6ds3_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) { + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + case ST_MASK_ID_GYRO: + return -EBUSY; + + default: + break; + } + } +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + return 0; +} + +static int st_lsm6ds3_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[sdata->sindex]) && + (indio_dev->buffer->length < 2 * ST_LSM6DS3_MAX_FIFO_LENGHT)) + return -EINVAL; + + sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); + if (!sdata->buffer_data) + return -ENOMEM; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (sdata->sindex == ST_MASK_ID_STEP_COUNTER) + iio_trigger_poll_chained(sdata->cdata->trig[ST_MASK_ID_STEP_COUNTER]); + + if (sdata->sindex == ST_MASK_ID_SIGN_MOTION) + sdata->cdata->sign_motion_event_ready = true; + + return 0; +} + +static int st_lsm6ds3_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->sindex == ST_MASK_ID_SIGN_MOTION) + sdata->cdata->sign_motion_event_ready = false; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + kfree(sdata->buffer_data); + + return err < 0 ? err : 0; +} + +static const struct iio_buffer_setup_ops st_lsm6ds3_buffer_setup_ops = { + .preenable = &st_lsm6ds3_buffer_preenable, + .postenable = &st_lsm6ds3_buffer_postenable, + .postdisable = &st_lsm6ds3_buffer_postdisable, +}; + +int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata) +{ + int err; + struct lsm6ds3_sensor_data *sdata; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_ACCEL], + NULL, &st_lsm6ds3_outdata_trigger_handler, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + return err; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_GYRO]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_GYRO], + NULL, &st_lsm6ds3_outdata_trigger_handler, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_accel; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_gyro; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER], + NULL, + &st_lsm6ds3_step_counter_trigger_handler, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_sign_motion; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_counter; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_TILT], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_detector; + + return 0; + +buffer_cleanup_step_detector: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]); +buffer_cleanup_step_counter: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]); +buffer_cleanup_sign_motion: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]); +buffer_cleanup_gyro: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +buffer_cleanup_accel: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + return err; +} + +void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +} diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_core.c new file mode 100644 index 000000000000..bfaf12980d5b --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_core.c @@ -0,0 +1,3209 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3 core driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "st_lsm6ds3.h" + +#define MS_TO_NS(msec) ((msec) * 1000 * 1000) + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif + +#define MIN_BNZ(a, b) (((a) < (b)) ? ((a == 0) ? \ + (b) : (a)) : ((b == 0) ? \ + (a) : (b))) + +/* COMMON VALUES FOR ACCEL-GYRO SENSORS */ +#define ST_LSM6DS3_WAI_ADDRESS 0x0f +#define ST_LSM6DS3_WAI_EXP 0x69 +#define ST_LSM6DS3_INT1_ADDR 0x0d +#define ST_LSM6DS3_INT2_ADDR 0x0e +#define ST_LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01 +#define ST_LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02 +#define ST_LSM6DS3_MD1_ADDR 0x5e +#define ST_LSM6DS3_ODR_LIST_NUM 6 +#define ST_LSM6DS3_ODR_POWER_OFF_VAL 0x00 +#define ST_LSM6DS3_ODR_13HZ_VAL 0x01 +#define ST_LSM6DS3_ODR_26HZ_VAL 0x02 +#define ST_LSM6DS3_ODR_52HZ_VAL 0x03 +#define ST_LSM6DS3_ODR_104HZ_VAL 0x04 +#define ST_LSM6DS3_ODR_208HZ_VAL 0x05 +#define ST_LSM6DS3_ODR_416HZ_VAL 0x06 +#define ST_LSM6DS3_FS_LIST_NUM 4 +#define ST_LSM6DS3_BDU_ADDR 0x12 +#define ST_LSM6DS3_BDU_MASK 0x40 +#define ST_LSM6DS3_EN_BIT 0x01 +#define ST_LSM6DS3_DIS_BIT 0x00 +#define ST_LSM6DS3_FUNC_EN_ADDR 0x19 +#define ST_LSM6DS3_FUNC_EN_MASK 0x04 +#define ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01 +#define ST_LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04 +#define ST_LSM6DS3_FUNC_CFG_REG2_MASK 0x80 +#define ST_LSM6DS3_FUNC_CFG_START1_ADDR 0x62 +#define ST_LSM6DS3_FUNC_CFG_START2_ADDR 0x63 +#define ST_LSM6DS3_SENSORHUB_ADDR 0x1a +#define ST_LSM6DS3_SENSORHUB_MASK 0x01 +#define ST_LSM6DS3_SENSORHUB_TRIG_MASK 0x10 +#define ST_LSM6DS3_TRIG_INTERNAL 0x00 +#define ST_LSM6DS3_TRIG_EXTERNAL 0x01 +#define ST_LSM6DS3_SELFTEST_ADDR 0x14 +#define ST_LSM6DS3_SELFTEST_ACCEL_MASK 0x03 +#define ST_LSM6DS3_SELFTEST_GYRO_MASK 0x0c +#define ST_LSM6DS3_SELF_TEST_DISABLED_VAL 0x00 +#define ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01 +#define ST_LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 +#define ST_LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 +#define ST_LSM6DS3_LIR_ADDR 0x58 +#define ST_LSM6DS3_LIR_MASK 0x01 +#define ST_LSM6DS3_TIMER_EN_ADDR 0x58 +#define ST_LSM6DS3_TIMER_EN_MASK 0x80 +#define ST_LSM6DS3_PEDOMETER_EN_ADDR 0x58 +#define ST_LSM6DS3_PEDOMETER_EN_MASK 0x40 +#define ST_LSM6DS3_INT2_ON_INT1_ADDR 0x13 +#define ST_LSM6DS3_INT2_ON_INT1_MASK 0x20 +#define ST_LSM6DS3_MIN_DURATION_MS 1638 +#define ST_LSM6DS3_ROUNDING_ADDR 0x16 +#define ST_LSM6DS3_ROUNDING_MASK 0x04 +#define ST_LSM6DS3_FIFO_MODE_ADDR 0x0a +#define ST_LSM6DS3_FIFO_MODE_MASK 0x07 +#define ST_LSM6DS3_FIFO_MODE_BYPASS 0x00 +#define ST_LSM6DS3_FIFO_MODE_CONTINUOS 0x06 +#define ST_LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08 +#define ST_LSM6DS3_FIFO_ODR_MAX 0x40 +#define ST_LSM6DS3_FIFO_DECIMATOR_ADDR 0x08 +#define ST_LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK 0x07 +#define ST_LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38 +#define ST_LSM6DS3_FIFO_DECIMATOR2_ADDR 0x09 +#define ST_LSM6DS3_FIFO_THR_L_ADDR 0x06 +#define ST_LSM6DS3_FIFO_THR_H_ADDR 0x07 +#define ST_LSM6DS3_FIFO_THR_MASK 0x0fff +#define ST_LSM6DS3_FIFO_THR_IRQ_MASK 0x08 +#define ST_LSM6DS3_RESET_ADDR 0x12 +#define ST_LSM6DS3_RESET_MASK 0x01 +#define ST_LSM6DS3_TEST_REG_ADDR 0x00 +#define ST_LSM6DS3_START_INJECT_XL_MASK 0x08 +#define ST_LSM6DS3_INJECT_XL_X_ADDR 0x06 +#define ST_LSM6DS3_NS_AT_25HZ 40000000LL +#define ST_LSM6DS3_26HZ_NS (38461538LL) +#define ST_LSM6DS3_SELFTEST_NA_MS "na" +#define ST_LSM6DS3_SELFTEST_FAIL_MS "fail" +#define ST_LSM6DS3_SELFTEST_PASS_MS "pass" + +/* CUSTOM VALUES FOR ACCEL SENSOR */ +#define ST_LSM6DS3_ACCEL_ODR_ADDR 0x10 +#define ST_LSM6DS3_ACCEL_ODR_MASK 0xf0 +#define ST_LSM6DS3_ACCEL_FS_ADDR 0x10 +#define ST_LSM6DS3_ACCEL_FS_MASK 0x0c +#define ST_LSM6DS3_ACCEL_FS_2G_VAL 0x00 +#define ST_LSM6DS3_ACCEL_FS_4G_VAL 0x02 +#define ST_LSM6DS3_ACCEL_FS_8G_VAL 0x03 +#define ST_LSM6DS3_ACCEL_FS_16G_VAL 0x01 +#define ST_LSM6DS3_ACCEL_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_LSM6DS3_ACCEL_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_LSM6DS3_ACCEL_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_LSM6DS3_ACCEL_FS_16G_GAIN IIO_G_TO_M_S_2(488000) +#define ST_LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28 +#define ST_LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DS3_ACCEL_STD_52HZ 1 +#define ST_LSM6DS3_ACCEL_STD_104HZ 2 +#define ST_LSM6DS3_ACCEL_STD_208HZ 3 +#define ST_LSM6DS3_SELFTEST_ACCEL_ADDR 0x10 +#define ST_LSM6DS3_SELFTEST_ACCEL_REG_VALUE 0x40 +#define ST_LSM6DS3_SELFTEST_ACCEL_MIN 1492 +#define ST_LSM6DS3_SELFTEST_ACCEL_MAX 27868 + +/* CUSTOM VALUES FOR GYRO SENSOR */ +#define ST_LSM6DS3_GYRO_ODR_ADDR 0x11 +#define ST_LSM6DS3_GYRO_ODR_MASK 0xf0 +#define ST_LSM6DS3_GYRO_FS_ADDR 0x11 +#define ST_LSM6DS3_GYRO_FS_MASK 0x0c +#define ST_LSM6DS3_GYRO_FS_250_VAL 0x00 +#define ST_LSM6DS3_GYRO_FS_500_VAL 0x01 +#define ST_LSM6DS3_GYRO_FS_1000_VAL 0x02 +#define ST_LSM6DS3_GYRO_FS_2000_VAL 0x03 +#define ST_LSM6DS3_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_LSM6DS3_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_LSM6DS3_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_LSM6DS3_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_LSM6DS3_GYRO_OUT_X_L_ADDR 0x22 +#define ST_LSM6DS3_GYRO_OUT_Y_L_ADDR 0x24 +#define ST_LSM6DS3_GYRO_OUT_Z_L_ADDR 0x26 +#define ST_LSM6DS3_GYRO_STD_13HZ 2 +#define ST_LSM6DS3_GYRO_STD_52HZ 3 +#define ST_LSM6DS3_GYRO_STD_104HZ 5 +#define ST_LSM6DS3_GYRO_STD_208HZ 8 +#define ST_LSM6DS3_SELFTEST_GYRO_ADDR 0x11 +#define ST_LSM6DS3_SELFTEST_GYRO_REG_VALUE 0x4c +#define ST_LSM6DS3_SELFTEST_GYRO_MIN 2142 +#define ST_LSM6DS3_SELFTEST_GYRO_MAX 10000 + +/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */ +#define ST_LSM6DS3_SIGN_MOTION_EN_ADDR 0x19 +#define ST_LSM6DS3_SIGN_MOTION_EN_MASK 0x01 + +/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ +#define ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK 0x80 + +/* CUSTOM VALUES FOR STEP COUNTER SENSOR */ +#define ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80 +#define ST_LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b +#define ST_LSM6DS3_STEP_COUNTER_RES_ADDR 0x19 +#define ST_LSM6DS3_STEP_COUNTER_RES_MASK 0x06 +#define ST_LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03 +#define ST_LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02 +#define ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15 +#define ST_LSM6DS3_STEP_COUNTER_THS_ADDR 0x0f +#define ST_LSM6DS3_STEP_COUNTER_THS_2G_VALUE (0x00 | 0x10) +#define ST_LSM6DS3_STEP_COUNTER_THS_4G_VALUE (0x80 | 0x08) + +/* CUSTOM VALUES FOR TILT SENSOR */ +#define ST_LSM6DS3_TILT_EN_ADDR 0x58 +#define ST_LSM6DS3_TILT_EN_MASK 0x20 +#define ST_LSM6DS3_TILT_DRDY_IRQ_MASK 0x02 + +#define ST_LSM6DS3_ACCEL_SUFFIX_NAME "accel" +#define ST_LSM6DS3_GYRO_SUFFIX_NAME "gyro" +#define ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME "step_c" +#define ST_LSM6DS3_STEP_DETECTOR_SUFFIX_NAME "step_d" +#define ST_LSM6DS3_SIGN_MOTION_SUFFIX_NAME "sign_motion" +#define ST_LSM6DS3_TILT_SUFFIX_NAME "tilt" + +#define ST_LSM6DS3_DEV_ATTR_SAMP_FREQ() \ + IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \ + st_lsm6ds3_sysfs_get_sampling_frequency, \ + st_lsm6ds3_sysfs_set_sampling_frequency) + +#define ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL() \ + IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \ + st_lsm6ds3_sysfs_sampling_frequency_avail) + +#define ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(name) \ + IIO_DEVICE_ATTR(name, S_IRUGO, \ + st_lsm6ds3_sysfs_scale_avail, NULL , 0); + +static struct st_lsm6ds3_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_lsm6ds3_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DS3_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_LSM6DS3_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +struct st_lsm6ds3_odr_reg { + unsigned int hz; + u8 value; +}; + +static struct st_lsm6ds3_odr_table { + u8 addr[2]; + u8 mask[2]; + struct st_lsm6ds3_odr_reg odr_avl[ST_LSM6DS3_ODR_LIST_NUM]; +} st_lsm6ds3_odr_table = { + .addr[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_ODR_ADDR, + .mask[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_ODR_MASK, + .addr[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_ODR_ADDR, + .mask[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_ODR_MASK, + .odr_avl[0] = { .hz = 13, .value = ST_LSM6DS3_ODR_13HZ_VAL }, + .odr_avl[1] = { .hz = 26, .value = ST_LSM6DS3_ODR_26HZ_VAL }, + .odr_avl[2] = { .hz = 52, .value = ST_LSM6DS3_ODR_52HZ_VAL }, + .odr_avl[3] = { .hz = 104, .value = ST_LSM6DS3_ODR_104HZ_VAL }, + .odr_avl[4] = { .hz = 208, .value = ST_LSM6DS3_ODR_208HZ_VAL }, + .odr_avl[5] = { .hz = 416, .value = ST_LSM6DS3_ODR_416HZ_VAL }, +}; + +struct st_lsm6ds3_fs_reg { + unsigned int gain; + u8 value; +}; + +static struct st_lsm6ds3_fs_table { + u8 addr; + u8 mask; + struct st_lsm6ds3_fs_reg fs_avl[ST_LSM6DS3_FS_LIST_NUM]; +} st_lsm6ds3_fs_table[ST_INDIO_DEV_NUM] = { + [ST_MASK_ID_ACCEL] = { + .addr = ST_LSM6DS3_ACCEL_FS_ADDR, + .mask = ST_LSM6DS3_ACCEL_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DS3_ACCEL_FS_2G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_2G_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DS3_ACCEL_FS_4G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_4G_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DS3_ACCEL_FS_8G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_8G_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DS3_ACCEL_FS_16G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_16G_VAL }, + }, + [ST_MASK_ID_GYRO] = { + .addr = ST_LSM6DS3_GYRO_FS_ADDR, + .mask = ST_LSM6DS3_GYRO_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DS3_GYRO_FS_250_GAIN, + .value = ST_LSM6DS3_GYRO_FS_250_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DS3_GYRO_FS_500_GAIN, + .value = ST_LSM6DS3_GYRO_FS_500_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DS3_GYRO_FS_1000_GAIN, + .value = ST_LSM6DS3_GYRO_FS_1000_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DS3_GYRO_FS_2000_GAIN, + .value = ST_LSM6DS3_GYRO_FS_2000_VAL }, + } +}; + +static const struct iio_event_spec singol_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, +}; + +const struct iio_event_spec lsm6ds3_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lsm6ds3_accel_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3_ACCEL_OUT_X_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3_ACCEL_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3_ACCEL_OUT_Z_L_ADDR, 's'), + ST_LSM6DS3_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6ds3_gyro_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3_GYRO_OUT_X_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3_GYRO_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3_GYRO_OUT_Z_L_ADDR, 's'), + ST_LSM6DS3_FLUSH_CHANNEL(IIO_ANGL_VEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6ds3_sign_motion_ch[] = { + { + .type = IIO_SIGN_MOTION, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3_step_c_ch[] = { + { + .type = IIO_STEP_COUNTER, + .modified = 0, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .address = ST_LSM6DS3_STEP_COUNTER_OUT_L_ADDR, + .scan_type = { + .sign = 'u', + .realbits = 64, + .storagebits = 64, + .endianness = IIO_LE, + }, + }, + ST_LSM6DS3_FLUSH_CHANNEL(IIO_STEP_COUNTER), + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3_step_d_ch[] = { + ST_LSM6DS3_FLUSH_CHANNEL(IIO_STEP_DETECTOR), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + +static const struct iio_chan_spec st_lsm6ds3_tilt_ch[] = { + ST_LSM6DS3_FLUSH_CHANNEL(IIO_TILT), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + + +int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = cdata->tf->read(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} +EXPORT_SYMBOL(st_lsm6ds3_write_data_with_mask); + +static inline int st_lsm6ds3_enable_embedded_page_regs(struct lsm6ds3_data *cdata, bool enable) +{ + u8 value = 0x00; + + if (enable) + value = ST_LSM6DS3_FUNC_CFG_REG2_MASK; + + return cdata->tf->write(cdata, ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, 1, &value, false); +} + +int st_lsm6ds3_write_embedded_registers(struct lsm6ds3_data *cdata, + u8 reg_addr, u8 *data, int len) +{ + int err = 0, err2, count = 0; + + mutex_lock(&cdata->bank_registers_lock); + + if (cdata->enable_digfunc_mask) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_DIS_BIT, false); + if (err < 0) { + mutex_unlock(&cdata->bank_registers_lock); + return err; + } + } + + udelay(100); + + err = st_lsm6ds3_enable_embedded_page_regs(cdata, true); + if (err < 0) + goto restore_digfunc; + + udelay(100); + + err = cdata->tf->write(cdata, reg_addr, len, data, false); + if (err < 0) + goto restore_bank_regs; + + err = st_lsm6ds3_enable_embedded_page_regs(cdata, false); + if (err < 0) + goto restore_digfunc; + + udelay(100); + + if (cdata->enable_digfunc_mask) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_EN_BIT, false); + if (err < 0) + goto restore_digfunc; + } + + mutex_unlock(&cdata->bank_registers_lock); + + return 0; + +restore_bank_regs: + do { + msleep(200); + err2 = st_lsm6ds3_enable_embedded_page_regs(cdata, false); + } while ((err2 < 0) && (count++ < 10)); + + if (count >= 10) + pr_err("not able to close embedded page registers. It make driver unstable!\n"); + +restore_digfunc: + if (cdata->enable_digfunc_mask) { + err2 = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_EN_BIT, false); + } + + mutex_unlock(&cdata->bank_registers_lock); + + return err; + +} + +static int lsm6ds3_set_watermark(struct lsm6ds3_data *cdata) +{ + int err; + u8 reg_value = 0; + u16 fifo_watermark; + unsigned int fifo_len, sip = 0, min_pattern = UINT_MAX; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_ACCEL] / + cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + } + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_GYRO].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_GYRO] / + cdata->fifo_output[ST_MASK_ID_GYRO].sip); + } + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_EXT0].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_EXT0] / + cdata->fifo_output[ST_MASK_ID_EXT0].sip); + } +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + if (sip == 0) + return 0; + + if (min_pattern == 0) + min_pattern = 1; + + min_pattern = MIN(min_pattern, ((unsigned int)ST_LSM6DS3_MAX_FIFO_THRESHOLD / sip)); + + fifo_len = min_pattern * sip * ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + fifo_watermark = (fifo_len / 2); + + if (fifo_watermark < (ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE / 2)) + fifo_watermark = ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE / 2; + + if (fifo_watermark != cdata->fifo_watermark) { + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_THR_H_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + fifo_watermark = (fifo_watermark & ST_LSM6DS3_FIFO_THR_MASK) | + ((reg_value & ~ST_LSM6DS3_FIFO_THR_MASK) << 8); + + err = cdata->tf->write(cdata, ST_LSM6DS3_FIFO_THR_L_ADDR, 2, + (u8 *)&fifo_watermark, true); + if (err < 0) + return err; + + cdata->fifo_watermark = fifo_watermark; + } + + return 0; +} + +int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm) +{ + int err; + u8 reg_value; + + switch (fm) { + case BYPASS: + reg_value = ST_LSM6DS3_FIFO_MODE_BYPASS; + break; + case CONTINUOS: + reg_value = ST_LSM6DS3_FIFO_MODE_CONTINUOS | ST_LSM6DS3_FIFO_ODR_MAX; + break; + default: + return -EINVAL; + } + + err = cdata->tf->write(cdata, ST_LSM6DS3_FIFO_MODE_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (fm != BYPASS) { + cdata->slower_counter = 0; + cdata->fifo_enable_timestamp = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = 0; + } + + cdata->fifo_status = fm; + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_set_fifo_mode); + +static int lsm6ds3_write_decimators(struct lsm6ds3_data *cdata, + u8 decimators[3]) +{ + int i; + u8 value[3], decimators_reg[2]; + + for (i = 0; i < 3; i++) { + switch (decimators[i]) { + case 0: + case 1: + case 2: + case 3: + case 4: + value[i] = decimators[i]; + break; + case 8: + value[i] = 0x05; + break; + case 16: + value[i] = 0x06; + break; + case 32: + value[i] = 0x07; + break; + default: + return -EINVAL; + } + } + + decimators_reg[0] = value[0] | (value[1] << 3); + decimators_reg[1] = value[2]; + + return cdata->tf->write(cdata, ST_LSM6DS3_FIFO_DECIMATOR_ADDR, + ARRAY_SIZE(decimators_reg), decimators_reg, true); +} + +static bool lsm6ds3_calculate_fifo_decimators(struct lsm6ds3_data *cdata, + u8 decimators[3], u8 samples_in_pattern[3], + unsigned int new_v_odr[ST_INDIO_DEV_NUM + 1], + unsigned int new_hw_odr[ST_INDIO_DEV_NUM + 1], + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1], + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1]) +{ + unsigned int trigger_odr; + u8 min_decimator, max_decimator = 0; + u8 accel_decimator = 0, gyro_decimator = 0, ext_decimator = 0; + + trigger_odr = new_hw_odr[ST_MASK_ID_ACCEL]; + if (trigger_odr < new_hw_odr[ST_MASK_ID_GYRO]) + trigger_odr = new_hw_odr[ST_MASK_ID_GYRO]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_ACCEL)) && + (new_v_odr[ST_MASK_ID_ACCEL] != 0) && cdata->accel_on) + accel_decimator = trigger_odr / new_v_odr[ST_MASK_ID_ACCEL]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_GYRO)) && + (new_v_odr[ST_MASK_ID_GYRO] != 0) && + (new_hw_odr[ST_MASK_ID_GYRO] > 0)) + gyro_decimator = trigger_odr / new_v_odr[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_EXT0)) && + (new_v_odr[ST_MASK_ID_EXT0] != 0) && cdata->magn_on) + ext_decimator = trigger_odr / new_v_odr[ST_MASK_ID_EXT0]; + + new_fifo_decimator[ST_MASK_ID_EXT0] = 1; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + new_fifo_decimator[ST_MASK_ID_ACCEL] = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = 1; + + if ((accel_decimator != 0) || (gyro_decimator != 0) || (ext_decimator != 0)) { + min_decimator = MIN_BNZ(MIN_BNZ(accel_decimator, gyro_decimator), ext_decimator); + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + if (min_decimator != 1) { + if ((accel_decimator / min_decimator) == 1) { + accel_decimator = 1; + new_fifo_decimator[ST_MASK_ID_ACCEL] = min_decimator; + } else if ((gyro_decimator / min_decimator) == 1) { + gyro_decimator = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = min_decimator; + } else if ((ext_decimator / min_decimator) == 1) { + ext_decimator = 1; + new_fifo_decimator[ST_MASK_ID_EXT0] = min_decimator; + } + min_decimator = 1; + } + if ((accel_decimator > 4) && (accel_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 3; + accel_decimator = 4; + } else if ((accel_decimator > 8) && (accel_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 7; + accel_decimator = 8; + } + if ((gyro_decimator > 4) && (gyro_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 3; + gyro_decimator = 4; + } else if ((gyro_decimator > 8) && (gyro_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 7; + gyro_decimator = 8; + } +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((ext_decimator > 4) && (ext_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 3; + ext_decimator = 4; + } else if ((ext_decimator > 8) && (ext_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 7; + ext_decimator = 8; + } +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + } + + decimators[0] = accel_decimator; + if (accel_decimator > 0) { + new_deltatime[ST_MASK_ID_ACCEL] = accel_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[0] = max_decimator / accel_decimator; + } else + samples_in_pattern[0] = 0; + + decimators[1] = gyro_decimator; + if (gyro_decimator > 0) { + new_deltatime[ST_MASK_ID_GYRO] = gyro_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[1] = max_decimator / gyro_decimator; + } else + samples_in_pattern[1] = 0; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + decimators[2] = ext_decimator; + if (ext_decimator > 0) { + new_deltatime[ST_MASK_ID_EXT0] = ext_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[2] = max_decimator / ext_decimator; + } else + samples_in_pattern[2] = 0; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (ext_decimator == cdata->hwfifo_decimator[ST_MASK_ID_EXT0]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + return false; + } + + return true; +} + +int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state) +{ + int err; + u16 *irq_mask = NULL; + u8 reg_addr, mask = 0, value; + u16 tmp_irq_enable_fifo_mask, tmp_irq_enable_accel_ext_mask; + + if (state) + value = ST_LSM6DS3_EN_BIT; + else + value = ST_LSM6DS3_DIS_BIT; + + tmp_irq_enable_fifo_mask = + sdata->cdata->irq_enable_fifo_mask & ~sdata->sindex; + tmp_irq_enable_accel_ext_mask = + sdata->cdata->irq_enable_accel_ext_mask & ~sdata->sindex; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = ST_LSM6DS3_INT1_ADDR; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_ACCEL]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_LSM6DS3_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; + case ST_MASK_ID_GYRO: + reg_addr = ST_LSM6DS3_INT1_ADDR; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_GYRO]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else + mask = ST_LSM6DS3_GYRO_DRDY_IRQ_MASK; + + break; + case ST_MASK_ID_SIGN_MOTION: + if (sdata->cdata->sensors_enabled & + BIT(ST_MASK_ID_STEP_DETECTOR)) + return 0; + + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_STEP_COUNTER: + reg_addr = ST_LSM6DS3_INT2_ADDR; + mask = ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_STEP_DETECTOR: + if (sdata->cdata->sensors_enabled & BIT(ST_MASK_ID_SIGN_MOTION)) + return 0; + + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_TILT: + reg_addr = ST_LSM6DS3_MD1_ADDR; + mask = ST_LSM6DS3_TILT_DRDY_IRQ_MASK; + break; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + reg_addr = ST_LSM6DS3_INT1_ADDR; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_LSM6DS3_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + default: + return -EINVAL; + } + + if (mask > 0) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); + if (err < 0) + return err; + } + + if (irq_mask != NULL) { + if (state) + *irq_mask |= BIT(sdata->sindex); + else + *irq_mask &= ~BIT(sdata->sindex); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_set_drdy_irq); + +static int st_lsm6ds3_set_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr, bool force) +{ + u8 reg_value; + int err, i = 0, n; + int64_t temp_last_timestamp[3] = { 0 }; + bool scan_odr = true, fifo_conf_changed; + unsigned int temp_v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int temp_hw_odr[ST_INDIO_DEV_NUM + 1]; + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1] = { 0 }; + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1] = { 0 }; + u8 fifo_decimator[3] = { 0 }, samples_in_pattern[3] = { 0 }; + u8 temp_num_samples[3] = { 0 }, temp_old_decimator[3] = { 1 }; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + + if (scan_odr) { + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_odr_table.odr_avl[i].hz == odr) + break; + } + if (i == ST_LSM6DS3_ODR_LIST_NUM) + return -EINVAL; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = st_lsm6ds3_odr_table.odr_avl[i].hz; + return 0; + } + } + + if (sdata->cdata->hw_odr[sdata->sindex] == st_lsm6ds3_odr_table.odr_avl[i].hz) + reg_value = 0xff; + else + reg_value = st_lsm6ds3_odr_table.odr_avl[i].value; + } else + reg_value = ST_LSM6DS3_ODR_POWER_OFF_VAL; + + if (sdata->cdata->sensors_use_fifo > 0) { + /* someone is using fifo */ + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + temp_v_odr[ST_MASK_ID_GYRO] = sdata->cdata->v_odr[ST_MASK_ID_GYRO]; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (force) + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->accel_odr_dependency[0]; + + temp_hw_odr[ST_MASK_ID_ACCEL] = odr; + temp_hw_odr[ST_MASK_ID_GYRO] = sdata->cdata->hw_odr[ST_MASK_ID_GYRO]; + } else { + if (!force) + temp_v_odr[ST_MASK_ID_GYRO] = odr; + + temp_hw_odr[ST_MASK_ID_GYRO] = odr; + temp_hw_odr[ST_MASK_ID_ACCEL] = sdata->cdata->hw_odr[ST_MASK_ID_ACCEL]; + } +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + temp_v_odr[ST_MASK_ID_EXT0] = sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + fifo_conf_changed = lsm6ds3_calculate_fifo_decimators(sdata->cdata, + fifo_decimator, samples_in_pattern, temp_v_odr, + temp_hw_odr, new_deltatime, new_fifo_decimator); + if (fifo_conf_changed) { + /* FIFO configuration changed, needs to write new decimators */ + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_lsm6ds3_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + err = st_lsm6ds3_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + err = lsm6ds3_write_decimators(sdata->cdata, fifo_decimator); + if (err < 0) + goto reenable_fifo_irq; + + if (reg_value != 0xff) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_208HZ; + break; + } + } + + sdata->cdata->hwfifo_decimator[ST_MASK_ID_ACCEL] = fifo_decimator[0]; + sdata->cdata->hwfifo_decimator[ST_MASK_ID_GYRO] = fifo_decimator[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator = new_fifo_decimator[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator = new_fifo_decimator[ST_MASK_ID_GYRO]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = new_fifo_decimator[ST_MASK_ID_ACCEL] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = new_fifo_decimator[ST_MASK_ID_GYRO] - 1; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip = samples_in_pattern[0]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip = samples_in_pattern[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default = new_deltatime[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + sdata->cdata->hwfifo_decimator[ST_MASK_ID_EXT0] = fifo_decimator[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator = new_fifo_decimator[ST_MASK_ID_EXT0]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = new_fifo_decimator[ST_MASK_ID_EXT0] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip = samples_in_pattern[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + err = lsm6ds3_set_watermark(sdata->cdata); + if (err < 0) + goto reenable_fifo_irq; + + if ((samples_in_pattern[0] > 0) || (samples_in_pattern[1] > 0) || (samples_in_pattern[2] > 0)) { + err = st_lsm6ds3_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_lsm6ds3_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_lsm6ds3_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_lsm6ds3_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } else { + /* FIFO configuration not changed */ + + if (reg_value == 0xff) { + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + return 0; + } + + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_lsm6ds3_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + err = st_lsm6ds3_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_208HZ; + break; + } + + if ((sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0)) { + err = st_lsm6ds3_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_lsm6ds3_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_lsm6ds3_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_lsm6ds3_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + } else { + /* no one is using FIFO */ + + disable_irq(sdata->cdata->irq); + + if ((odr != 0) && (sdata->cdata->hw_odr[sdata->sindex] == st_lsm6ds3_odr_table.odr_avl[i].hz)) { + if (sdata->sindex == ST_MASK_ID_ACCEL) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + + return 0; + } + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) { + enable_irq(sdata->cdata->irq); + return err; + } + + if (!force) + sdata->cdata->v_odr[sdata->sindex] = st_lsm6ds3_odr_table.odr_avl[i].hz; + + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = st_lsm6ds3_odr_table.odr_avl[i].hz; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (sdata->cdata->hw_odr[sdata->sindex]) { + case 13: + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3_ACCEL_STD_208HZ; + break; + } + } + + switch (sdata->cdata->hw_odr[ST_MASK_ID_GYRO]) { + case 13: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_13HZ; + break; + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3_GYRO_STD_208HZ; + break; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (sdata->cdata->hw_odr[sdata->sindex] > 0) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } else { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = 1; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = 1; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + sdata->cdata->trigger_odr = sdata->cdata->hw_odr[0] > sdata->cdata->hw_odr[1] ? sdata->cdata->hw_odr[0] : sdata->cdata->hw_odr[1]; + + return 0; + +reenable_fifo_irq: + enable_irq(sdata->cdata->irq); + return err; +} + +/* + * Enable / disable accelerometer + */ +static int lsm6ds3_enable_accel(struct lsm6ds3_data *cdata, enum st_mask_id id, int min_odr) +{ + int odr; + struct lsm6ds3_sensor_data *sdata_accel = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + switch (id) { + case ST_MASK_ID_ACCEL: + cdata->accel_odr_dependency[0] = min_odr; + if (min_odr > 0) + cdata->accel_on = true; + else + cdata->accel_on = false; + + break; + case ST_MASK_ID_SENSOR_HUB: + cdata->accel_odr_dependency[1] = min_odr; + if (min_odr > 0) + cdata->magn_on = true; + else + cdata->magn_on = false; + + break; + case ST_MASK_ID_DIGITAL_FUNC: + cdata->accel_odr_dependency[2] = min_odr; + break; + default: + return -EINVAL; + } + + if (cdata->accel_odr_dependency[0] > cdata->accel_odr_dependency[1]) + odr = cdata->accel_odr_dependency[0]; + else + odr = cdata->accel_odr_dependency[1]; + + if (cdata->accel_odr_dependency[2] > odr) + odr = cdata->accel_odr_dependency[2]; + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + if (cdata->injection_mode) + return 0; +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + return st_lsm6ds3_set_odr(sdata_accel, odr, true); +} + +/* + * Enable / disable digital func + */ +static int lsm6ds3_enable_digital_func(struct lsm6ds3_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_digfunc_mask == 0) { + err = lsm6ds3_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 26); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask |= BIT(id); + } else { + if ((cdata->enable_digfunc_mask & ~BIT(id)) == 0) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 0); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask &= ~BIT(id); + + } + + return 0; +} + +/* + * Enable / disable HW pedometer + */ +static int lsm6ds3_enable_pedometer(struct lsm6ds3_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_pedometer_mask == 0) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_digital_func(cdata, + true, ST_MASK_ID_HW_PEDOMETER); + if (err < 0) + return err; + } else { + if (id == ST_MASK_ID_SIGN_MOTION) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } + } + cdata->enable_pedometer_mask |= BIT(id); + } else { + if ((cdata->enable_pedometer_mask & ~BIT(id)) == 0) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_digital_func(cdata, + false, ST_MASK_ID_HW_PEDOMETER); + if (err < 0) + return err; + } + cdata->enable_pedometer_mask &= ~BIT(id); + } + + return 0; +} + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +int st_lsm6ds3_enable_sensor_hub(struct lsm6ds3_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_sensorhub_mask == 0) { + err = lsm6ds3_enable_digital_func(cdata, + true, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + + err = lsm6ds3_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_SENSORHUB_ADDR, + ST_LSM6DS3_SENSORHUB_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } else + err = lsm6ds3_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask |= BIT(id); + } else { + if ((cdata->enable_sensorhub_mask & ~BIT(id)) == 0) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_SENSORHUB_ADDR, + ST_LSM6DS3_SENSORHUB_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_accel(cdata, + ST_MASK_ID_SENSOR_HUB, 0); + if (err < 0) + return err; + + err = lsm6ds3_enable_digital_func(cdata, + false, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + } else + err = lsm6ds3_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask &= ~BIT(id); + } + + return err < 0 ? err : 0; +} +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + err = lsm6ds3_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, + enable ? sdata->cdata->v_odr[ST_MASK_ID_ACCEL] : 0); + if (err < 0) + return 0; + + break; + case ST_MASK_ID_GYRO: + err = st_lsm6ds3_set_odr(sdata, enable ? + sdata->cdata->v_odr[ST_MASK_ID_GYRO] : 0, true); + if (err < 0) + return err; + + break; + case ST_MASK_ID_SIGN_MOTION: + if (enable) + reg_value = ST_LSM6DS3_EN_BIT; + else + reg_value = ST_LSM6DS3_DIS_BIT; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_SIGN_MOTION_EN_ADDR, + ST_LSM6DS3_SIGN_MOTION_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_SIGN_MOTION); + if (err < 0) + return err; + + break; + case ST_MASK_ID_STEP_COUNTER: + if (enable) + reg_value = ST_LSM6DS3_EN_BIT; + else + reg_value = ST_LSM6DS3_DIS_BIT; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TIMER_EN_ADDR, + ST_LSM6DS3_TIMER_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_STEP_COUNTER); + if (err < 0) + return err; + + break; + case ST_MASK_ID_STEP_DETECTOR: + err = lsm6ds3_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_STEP_DETECTOR); + if (err < 0) + return err; + + break; + case ST_MASK_ID_TILT: + if (enable) + reg_value = ST_LSM6DS3_EN_BIT; + else + reg_value = ST_LSM6DS3_DIS_BIT; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TILT_EN_ADDR, + ST_LSM6DS3_TILT_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6ds3_enable_digital_func(sdata->cdata, + enable, ST_MASK_ID_TILT); + if (err < 0) + return err; + + break; + default: + return -EINVAL; + } + + if (buffer) { + err = st_lsm6ds3_set_drdy_irq(sdata, enable); + if (err < 0) + return err; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; +} + +static int st_lsm6ds3_set_fs(struct lsm6ds3_sensor_data *sdata, + unsigned int gain) +{ + int err, i; + u8 pedometer_reg_value; + + for (i = 0; i < ST_LSM6DS3_FS_LIST_NUM; i++) { + if (st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].gain == gain) + break; + } + if (i == ST_LSM6DS3_FS_LIST_NUM) + return -EINVAL; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_fs_table[sdata->sindex].addr, + st_lsm6ds3_fs_table[sdata->sindex].mask, + st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].value, + true); + if (err < 0) + return err; + + sdata->c_gain[0] = gain; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (i == 0) + pedometer_reg_value = ST_LSM6DS3_STEP_COUNTER_THS_2G_VALUE; + else + pedometer_reg_value = ST_LSM6DS3_STEP_COUNTER_THS_4G_VALUE; + + st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_STEP_COUNTER_THS_ADDR, + &pedometer_reg_value, 1); + } + + return 0; +} + +static int st_lsm6ds3_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[ST_LSM6DS3_BYTE_FOR_CHANNEL]; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) + msleep(40); + + if (sdata->sindex == ST_MASK_ID_GYRO) + msleep(120); + + err = sdata->cdata->tf->read(sdata->cdata, ch->address, + ST_LSM6DS3_BYTE_FOR_CHANNEL, outdata, true); + if (err < 0) { + st_lsm6ds3_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + st_lsm6ds3_set_enable(sdata, false, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[0]; + return IIO_VAL_INT_PLUS_NANO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6ds3_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long mask) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = st_lsm6ds3_set_fs(sdata, val2); + mutex_unlock(&indio_dev->mlock); + break; + default: + return -EINVAL; + } + + return err < 0 ? err : 0; +} + +static int st_lsm6ds3_reset_steps(struct lsm6ds3_data *cdata) +{ + int err; + u8 reg_value = 0x00; + + err = cdata->tf->read(cdata, + ST_LSM6DS3_STEP_COUNTER_RES_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (reg_value & ST_LSM6DS3_FUNC_EN_MASK) + reg_value = ST_LSM6DS3_STEP_COUNTER_RES_FUNC_EN; + else + reg_value = ST_LSM6DS3_DIS_BIT; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_STEP_COUNTER_RES_ADDR, + ST_LSM6DS3_STEP_COUNTER_RES_MASK, + ST_LSM6DS3_STEP_COUNTER_RES_ALL_EN, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_STEP_COUNTER_RES_ADDR, + ST_LSM6DS3_STEP_COUNTER_RES_MASK, + reg_value, true); + if (err < 0) + return err; + + cdata->reset_steps = true; + + return 0; +} + +static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata) +{ + int err; + u8 default_reg_value = ST_LSM6DS3_RESET_MASK; + + err = cdata->tf->write(cdata, ST_LSM6DS3_RESET_ADDR, 1, + &default_reg_value, true); + if (err < 0) + return err; + + msleep(200); + + /* Latch interrupts */ + err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_LIR_ADDR, + ST_LSM6DS3_LIR_MASK, ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + /* Enable BDU for sensors data */ + err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_BDU_ADDR, + ST_LSM6DS3_BDU_MASK, ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_ROUNDING_ADDR, + ST_LSM6DS3_ROUNDING_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + /* Redirect INT2 on INT1, all interrupt will be available on INT1 */ + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_INT2_ON_INT1_ADDR, + ST_LSM6DS3_INT2_ON_INT1_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_reset_steps(cdata); + if (err < 0) + return err; + + default_reg_value = 0x00; + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR, + &default_reg_value, 1); + if (err < 0) + return err; + + default_reg_value = ST_LSM6DS3_STEP_COUNTER_THS_2G_VALUE; + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_STEP_COUNTER_THS_ADDR, + &default_reg_value, 1); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6ds3_set_selftest(struct lsm6ds3_sensor_data *sdata, int index) +{ + u8 mode, mask; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + mask = ST_LSM6DS3_SELFTEST_ACCEL_MASK; + mode = st_lsm6ds3_selftest_table[index].accel_value; + break; + case ST_MASK_ID_GYRO: + mask = ST_LSM6DS3_SELFTEST_GYRO_MASK; + mode = st_lsm6ds3_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_SELFTEST_ADDR, mask, mode, true); +} + +static ssize_t st_lsm6ds3_sysfs_set_max_delivery_rate(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u8 duration; + int err; + unsigned int max_delivery_rate; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtouint(buf, 10, &max_delivery_rate); + if (err < 0) + return -EINVAL; + + if (max_delivery_rate == sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER]) + return size; + + duration = max_delivery_rate / ST_LSM6DS3_MIN_DURATION_MS; + + err = st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR, + &duration, 1); + if (err < 0) + return err; + + sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER] = max_delivery_rate; + + return size; +} + +static ssize_t st_lsm6ds3_sysfs_get_max_delivery_rate(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", + sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER]); +} + +static ssize_t st_lsm6ds3_sysfs_reset_counter(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + err = st_lsm6ds3_reset_steps(sdata->cdata); + if (err < 0) + return err; + + return size; +} + +static ssize_t st_lsm6ds3_sysfs_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_lsm6ds3_sysfs_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + + mutex_lock(&sdata->cdata->odr_lock); +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + if (!((sdata->sindex & ST_MASK_ID_ACCEL) && + sdata->cdata->injection_mode)) { + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_lsm6ds3_set_odr(sdata, odr, false); + } +#else /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + if (sdata->cdata->v_odr[sdata->sindex] != odr) { + if ((sdata->sindex == ST_MASK_ID_ACCEL) && (sdata->cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL))) + err = lsm6ds3_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, odr); + else + err = st_lsm6ds3_set_odr(sdata, odr, false); + } +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + mutex_unlock(&sdata->cdata->odr_lock); + + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6ds3_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lsm6ds3_odr_table.odr_avl[i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6ds3_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + for (i = 0; i < ST_LSM6DS3_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6ds3_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6ds3_selftest_table[1].string_mode, + st_lsm6ds3_selftest_table[2].string_mode); +} + +static ssize_t st_lsm6ds3_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + result = sdata->cdata->accel_selftest_status; + break; + case ST_MASK_ID_GYRO: + result = sdata->cdata->gyro_selftest_status; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_LSM6DS3_SELFTEST_NA_MS; + else if (result < 0) + message = ST_LSM6DS3_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_LSM6DS3_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_lsm6ds3_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i, n; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + u8 reg_status, reg_addr, temp_reg_status, outdata[6]; + int x = 0, y = 0, z = 0, x_selftest = 0, y_selftest = 0, z_selftest = 0; + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + sdata->cdata->accel_selftest_status = 0; + break; + case ST_MASK_ID_GYRO: + sdata->cdata->gyro_selftest_status = 0; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + for (n = 1; n < ARRAY_SIZE(st_lsm6ds3_selftest_table); n++) { + if (strncmp(buf, st_lsm6ds3_selftest_table[n].string_mode, + size - 2) == 0) + break; + } + if (n == ARRAY_SIZE(st_lsm6ds3_selftest_table)) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = ST_LSM6DS3_SELFTEST_ACCEL_ADDR; + temp_reg_status = ST_LSM6DS3_SELFTEST_ACCEL_REG_VALUE; + break; + case ST_MASK_ID_GYRO: + reg_addr = ST_LSM6DS3_SELFTEST_GYRO_ADDR; + temp_reg_status = ST_LSM6DS3_SELFTEST_GYRO_REG_VALUE; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = sdata->cdata->tf->read(sdata->cdata, + reg_addr, 1, ®_status, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, &temp_reg_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 20; + y += ((s16)*(u16 *)&outdata[2]) / 20; + z += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = st_lsm6ds3_set_selftest(sdata, n); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest enabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 20; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 20; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, ®_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = st_lsm6ds3_set_selftest(sdata, 0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + if ((abs(x_selftest - x) < ST_LSM6DS3_SELFTEST_ACCEL_MIN) || + (abs(x_selftest - x) > ST_LSM6DS3_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_LSM6DS3_SELFTEST_ACCEL_MIN) || + (abs(y_selftest - y) > ST_LSM6DS3_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_LSM6DS3_SELFTEST_ACCEL_MIN) || + (abs(z_selftest - z) > ST_LSM6DS3_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->accel_selftest_status = 1; + break; + case ST_MASK_ID_GYRO: + if ((abs(x_selftest - x) < ST_LSM6DS3_SELFTEST_GYRO_MIN) || + (abs(x_selftest - x) > ST_LSM6DS3_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_LSM6DS3_SELFTEST_GYRO_MIN) || + (abs(y_selftest - y) > ST_LSM6DS3_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_LSM6DS3_SELFTEST_GYRO_MIN) || + (abs(z_selftest - z) > ST_LSM6DS3_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->gyro_selftest_status = 1; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + +selftest_failure: + mutex_unlock(&sdata->cdata->odr_lock); + + return size; +} + +ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u64 sensor_last_timestamp, event_type = 0; + int stype = 0; + u64 timestamp_flush = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_lock(&sdata->cdata->odr_lock); + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = + sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + st_lsm6ds3_read_fifo(sdata->cdata, true); + + if (sensor_last_timestamp == + sdata->cdata->fifo_output[sdata->sindex].timestamp_p) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + timestamp_flush = sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + enable_irq(sdata->cdata->irq); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + stype = IIO_ACCEL; + break; + + case ST_MASK_ID_GYRO: + stype = IIO_ANGL_VEL; + break; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + stype = IIO_MAGN; + break; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + } + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(stype, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + timestamp_flush); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +ssize_t st_lsm6ds3_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_enabled[sdata->sindex]); +} + +ssize_t st_lsm6ds3_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + bool enable = false; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->spi_connection) { + dev_info(sdata->cdata->dev, + "FIFO not supported with SPI connection.\n"); + return -EINVAL; + } + + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + err = -EBUSY; + goto set_hwfifo_enabled_unlock_mutex; + } + + err = strtobool(buf, &enable); + if (err < 0) + goto set_hwfifo_enabled_unlock_mutex; + + mutex_lock(&sdata->cdata->odr_lock); + + sdata->cdata->hwfifo_enabled[sdata->sindex] = enable; + + if (enable) + sdata->cdata->sensors_use_fifo |= BIT(sdata->sindex); + else + sdata->cdata->sensors_use_fifo &= ~BIT(sdata->sindex); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; + +set_hwfifo_enabled_unlock_mutex: + mutex_unlock(&indio_dev->mlock); + return err; +} + +ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_watermark[sdata->sindex]); +} + +ssize_t st_lsm6ds3_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err = 0, watermark = 0, old_watermark; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > ST_LSM6DS3_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) && + (sdata->cdata->sensors_use_fifo & BIT(sdata->sindex))) { + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) + st_lsm6ds3_read_fifo(sdata->cdata, true); + + old_watermark = sdata->cdata->hwfifo_watermark[sdata->sindex]; + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + err = lsm6ds3_set_watermark(sdata->cdata); + if (err < 0) + sdata->cdata->hwfifo_watermark[sdata->sindex] = old_watermark; + + enable_irq(sdata->cdata->irq); + } else + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : size; +} + +ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", ST_LSM6DS3_MAX_FIFO_LENGHT); +} + +ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION +static ssize_t st_lsm6ds3_sysfs_set_injection_mode(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, start; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = kstrtoint(buf, 10, &start); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + mutex_lock(&sdata->cdata->odr_lock); + + if (start == 0) { + hrtimer_cancel(&sdata->cdata->injection_timer); + + /* End injection */ + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TEST_REG_ADDR, + ST_LSM6DS3_START_INJECT_XL_MASK, 0, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + /* Force accel ODR to 26Hz if dependencies are enabled */ + if (sdata->cdata->sensors_enabled > 0) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + st_lsm6ds3_odr_table.odr_avl[1].value, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + } + + sdata->cdata->injection_mode = false; + } else { + sdata->cdata->last_injection_timestamp = 0; + sdata->cdata->injection_samples = 0; + + /* Force accel ODR to 26Hz */ + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + st_lsm6ds3_odr_table.odr_avl[1].value, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + /* Set start injection */ + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TEST_REG_ADDR, + ST_LSM6DS3_START_INJECT_XL_MASK, 1, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + sdata->cdata->injection_mode = true; + } + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static ssize_t st_lsm6ds3_sysfs_get_injection_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->injection_mode); +} + +static ssize_t st_lsm6ds3_sysfs_upload_xl_data(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int i; + u8 sample[3]; + s64 timestamp; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (!sdata->cdata->injection_mode) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + for (i = 0; i < 3; i++) + sample[i] = *(s16 *)(&buf[i * 2]) >> 8; + + timestamp = *(s64 *)(buf + ALIGN(6, sizeof(s64))); + + if (timestamp < sdata->cdata->last_injection_timestamp + + ST_LSM6DS3_NS_AT_25HZ) { + mutex_unlock(&indio_dev->mlock); + return size; + } + + while (sdata->cdata->injection_samples >= 10) + msleep(200); + + spin_lock(&sdata->cdata->injection_spinlock); + + memcpy(&sdata->cdata->injection_data[ + sdata->cdata->injection_samples * 3], sample, 3); + sdata->cdata->injection_samples++; + + spin_unlock(&sdata->cdata->injection_spinlock); + + sdata->cdata->last_injection_timestamp = timestamp; + + if (sdata->cdata->injection_samples >= 8) + hrtimer_start(&sdata->cdata->injection_timer, + ktime_set(0, ST_LSM6DS3_26HZ_NS), HRTIMER_MODE_REL); + + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static void st_lsm6ds3_injection_work(struct work_struct *work) +{ + int i, err; + struct lsm6ds3_data *cdata; + + cdata = container_of(work, struct lsm6ds3_data, injection_work); + + if (cdata->injection_samples == 0) + return; + + err = cdata->tf->write(cdata, ST_LSM6DS3_INJECT_XL_X_ADDR, + 3, cdata->injection_data, false); + if (err < 0) + return; + + spin_lock(&cdata->injection_spinlock); + + for (i = 0; i < cdata->injection_samples - 1; i++) + memcpy(&cdata->injection_data[i * 3], + &cdata->injection_data[(i + 1) * 3], 3); + + cdata->injection_samples--; + + spin_unlock(&cdata->injection_spinlock); +} + +static enum hrtimer_restart st_lsm6ds3_injection_timer_func( + struct hrtimer *timer) +{ + ktime_t now; + struct lsm6ds3_data *cdata; + + cdata = container_of(timer, struct lsm6ds3_data, injection_timer); + + now = hrtimer_cb_get_time(timer); + hrtimer_forward(timer, now, ktime_set(0, ST_LSM6DS3_26HZ_NS)); + + schedule_work(&cdata->injection_work); + + return HRTIMER_RESTART; +} + +static ssize_t st_lsm6ds3_sysfs_get_injection_sensors(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", "lsm6ds3_accel"); +} +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + +ssize_t st_lsm6ds3_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + struct lsm6ds3_data *cdata = sdata->cdata; + + return scnprintf(buf, PAGE_SIZE, "%u\n", cdata->module_id); +} + +static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ(); +static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL(); +static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); +static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_anglvel_scale_available); + +static ST_LSM6DS3_HWFIFO_ENABLED(); +static ST_LSM6DS3_HWFIFO_WATERMARK(); +static ST_LSM6DS3_HWFIFO_WATERMARK_MIN(); +static ST_LSM6DS3_HWFIFO_WATERMARK_MAX(); +static ST_LSM6DS3_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(reset_counter, S_IWUSR, + NULL, st_lsm6ds3_sysfs_reset_counter, 0); + +static IIO_DEVICE_ATTR(max_delivery_rate, S_IWUSR | S_IRUGO, + st_lsm6ds3_sysfs_get_max_delivery_rate, + st_lsm6ds3_sysfs_set_max_delivery_rate, 0); + +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_lsm6ds3_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_lsm6ds3_sysfs_get_selftest_status, + st_lsm6ds3_sysfs_start_selftest, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6ds3_get_module_id, NULL, 0); + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION +static IIO_DEVICE_ATTR(injection_mode, S_IWUSR | S_IRUGO, + st_lsm6ds3_sysfs_get_injection_mode, + st_lsm6ds3_sysfs_set_injection_mode, 0); + +static IIO_DEVICE_ATTR(in_accel_injection_raw, S_IWUSR, NULL, + st_lsm6ds3_sysfs_upload_xl_data, 0); + +static IIO_DEVICE_ATTR(injection_sensors, S_IRUGO, + st_lsm6ds3_sysfs_get_injection_sensors, + NULL, 0); +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + +static int st_lsm6ds3_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == IIO_CHAN_INFO_SCALE) { + if ((chan->type == IIO_ANGL_VEL) || + (chan->type == IIO_ACCEL)) + return IIO_VAL_INT_PLUS_NANO; + } + + return -EINVAL; +} + +static struct attribute *st_lsm6ds3_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + &iio_dev_attr_injection_mode.dev_attr.attr, + &iio_dev_attr_in_accel_injection_raw.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3_accel_attribute_group = { + .attrs = st_lsm6ds3_accel_attributes, +}; + +static const struct iio_info st_lsm6ds3_accel_info = { + .attrs = &st_lsm6ds3_accel_attribute_group, + .read_raw = &st_lsm6ds3_read_raw, + .write_raw = &st_lsm6ds3_write_raw, + .write_raw_get_fmt = st_lsm6ds3_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6ds3_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.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_lsm6ds3_gyro_attribute_group = { + .attrs = st_lsm6ds3_gyro_attributes, +}; + +static const struct iio_info st_lsm6ds3_gyro_info = { + .attrs = &st_lsm6ds3_gyro_attribute_group, + .read_raw = &st_lsm6ds3_read_raw, + .write_raw = &st_lsm6ds3_write_raw, + .write_raw_get_fmt = st_lsm6ds3_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6ds3_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + NULL, +}; + +static const struct attribute_group st_lsm6ds3_sign_motion_attribute_group = { + .attrs = st_lsm6ds3_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6ds3_sign_motion_info = { + .attrs = &st_lsm6ds3_sign_motion_attribute_group, +}; + +static struct attribute *st_lsm6ds3_step_c_attributes[] = { + &iio_dev_attr_reset_counter.dev_attr.attr, + &iio_dev_attr_max_delivery_rate.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3_step_c_attribute_group = { + .attrs = st_lsm6ds3_step_c_attributes, +}; + +static const struct iio_info st_lsm6ds3_step_c_info = { + .attrs = &st_lsm6ds3_step_c_attribute_group, + .read_raw = &st_lsm6ds3_read_raw, +}; + +static struct attribute *st_lsm6ds3_step_d_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3_step_d_attribute_group = { + .attrs = st_lsm6ds3_step_d_attributes, +}; + +static const struct iio_info st_lsm6ds3_step_d_info = { + .attrs = &st_lsm6ds3_step_d_attribute_group, +}; + +static struct attribute *st_lsm6ds3_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3_tilt_attribute_group = { + .attrs = st_lsm6ds3_tilt_attributes, +}; + +static const struct iio_info st_lsm6ds3_tilt_info = { + .attrs = &st_lsm6ds3_tilt_attribute_group, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops st_lsm6ds3_trigger_ops = { + .set_trigger_state = ST_LSM6DS3_TRIGGER_SET_STATE, +}; +#define ST_LSM6DS3_TRIGGER_OPS (&st_lsm6ds3_trigger_ops) +#else +#define ST_LSM6DS3_TRIGGER_OPS NULL +#endif + +static void st_lsm6ds3_get_properties(struct lsm6ds3_data *cdata) +{ + if (device_property_read_u32(cdata->dev, "st,module_id", + &cdata->module_id)) { + cdata->module_id = 1; + } +} + +int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq) +{ + u8 wai = 0x00; + int i, n, err; + struct lsm6ds3_sensor_data *sdata; + + mutex_init(&cdata->bank_registers_lock); + mutex_init(&cdata->fifo_lock); + mutex_init(&cdata->tb.buf_lock); + mutex_init(&cdata->odr_lock); + + cdata->fifo_watermark = 0; + cdata->fifo_status = BYPASS; + cdata->enable_digfunc_mask = 0; + cdata->enable_pedometer_mask = 0; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + cdata->enable_sensorhub_mask = 0; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + cdata->irq_enable_fifo_mask = 0; + cdata->irq_enable_accel_ext_mask = 0; + + for (i = 0; i < ST_INDIO_DEV_NUM + 1; i++) { + cdata->hw_odr[i] = 0; + cdata->v_odr[i] = 13; + cdata->hwfifo_enabled[i] = false; + cdata->hwfifo_decimator[i] = 0; + cdata->hwfifo_watermark[i] = 1; + cdata->nofifo_decimation[i].decimator = 1; + cdata->nofifo_decimation[i].num_samples = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].decimator = 1; + cdata->fifo_output[i].timestamp_p = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].initialized = false; + } + + cdata->sensors_use_fifo = 0; + cdata->sensors_enabled = 0; + + cdata->gyro_selftest_status = 0; + cdata->accel_selftest_status = 0; + + cdata->accel_on = false; + cdata->magn_on = false; + + cdata->reset_steps = false; + cdata->sign_motion_event_ready = false; + cdata->num_steps = 0; + + cdata->accel_odr_dependency[0] = 0; + cdata->accel_odr_dependency[1] = 0; + cdata->accel_odr_dependency[2] = 0; + + cdata->trigger_odr = 0; + + cdata->fifo_data = kmalloc(ST_LSM6DS3_MAX_FIFO_SIZE * + sizeof(u8), GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + cdata->injection_mode = false; + cdata->last_injection_timestamp = 0; + + INIT_WORK(&cdata->injection_work, &st_lsm6ds3_injection_work); + hrtimer_init(&cdata->injection_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + cdata->injection_timer.function = &st_lsm6ds3_injection_timer_func; + spin_lock_init(&cdata->injection_spinlock); +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + goto free_fifo_data; + } + if (wai != ST_LSM6DS3_WAI_EXP) { + dev_err(cdata->dev, + "Who-Am-I value not valid. Expected %x, Found %x\n", + ST_LSM6DS3_WAI_EXP, wai); + err = -ENODEV; + goto free_fifo_data; + } + + st_lsm6ds3_get_properties(cdata); + + if (irq > 0) { + cdata->irq = irq; + dev_info(cdata->dev, "driver use DRDY int pin 1.\n"); + } else { + err = -EINVAL; + dev_err(cdata->dev, + "DRDY not available, current implementation needs irq!\n"); + goto free_fifo_data; + } + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + cdata->indio_dev[i] = devm_iio_device_alloc(cdata->dev, + sizeof(struct lsm6ds3_sensor_data)); + if (!cdata->indio_dev[i]) { + err = -ENOMEM; + goto free_fifo_data; + } + + sdata = iio_priv(cdata->indio_dev[i]); + sdata->cdata = cdata; + sdata->sindex = i; + + switch (i) { + case ST_MASK_ID_ACCEL: + sdata->data_out_reg = st_lsm6ds3_accel_ch[0].address; + cdata->v_odr[i] = st_lsm6ds3_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_lsm6ds3_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_GYRO: + sdata->data_out_reg = st_lsm6ds3_gyro_ch[0].address; + cdata->v_odr[i] = st_lsm6ds3_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_lsm6ds3_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_STEP_COUNTER: + sdata->data_out_reg = st_lsm6ds3_step_c_ch[0].address; + sdata->num_data_channels = 1; + break; + + default: + sdata->num_data_channels = 0; + break; + } + + cdata->indio_dev[i]->modes = INDIO_DIRECT_MODE; + } + + cdata->indio_dev[ST_MASK_ID_ACCEL]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_ACCEL_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_ACCEL]->info = &st_lsm6ds3_accel_info; + cdata->indio_dev[ST_MASK_ID_ACCEL]->channels = st_lsm6ds3_accel_ch; + cdata->indio_dev[ST_MASK_ID_ACCEL]->num_channels = + ARRAY_SIZE(st_lsm6ds3_accel_ch); + + cdata->indio_dev[ST_MASK_ID_GYRO]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_GYRO_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_GYRO]->info = &st_lsm6ds3_gyro_info; + cdata->indio_dev[ST_MASK_ID_GYRO]->channels = st_lsm6ds3_gyro_ch; + cdata->indio_dev[ST_MASK_ID_GYRO]->num_channels = + ARRAY_SIZE(st_lsm6ds3_gyro_ch); + + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_SIGN_MOTION_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->info = + &st_lsm6ds3_sign_motion_info; + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->channels = + st_lsm6ds3_sign_motion_ch; + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->num_channels = + ARRAY_SIZE(st_lsm6ds3_sign_motion_ch); + + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->info = + &st_lsm6ds3_step_c_info; + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->channels = + st_lsm6ds3_step_c_ch; + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->num_channels = + ARRAY_SIZE(st_lsm6ds3_step_c_ch); + + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_STEP_DETECTOR_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->info = + &st_lsm6ds3_step_d_info; + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->channels = + st_lsm6ds3_step_d_ch; + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->num_channels = + ARRAY_SIZE(st_lsm6ds3_step_d_ch); + + cdata->indio_dev[ST_MASK_ID_TILT]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_TILT_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_TILT]->info = &st_lsm6ds3_tilt_info; + cdata->indio_dev[ST_MASK_ID_TILT]->channels = st_lsm6ds3_tilt_ch; + cdata->indio_dev[ST_MASK_ID_TILT]->num_channels = + ARRAY_SIZE(st_lsm6ds3_tilt_ch); + + err = st_lsm6ds3_init_sensor(cdata); + if (err < 0) + goto free_fifo_data; + + err = st_lsm6ds3_allocate_rings(cdata); + if (err < 0) + goto free_fifo_data; + + if (irq > 0) { + err = st_lsm6ds3_allocate_triggers(cdata, + ST_LSM6DS3_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_device_register(cdata->indio_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + if (strcmp(cdata->name, LSM6DS33_DEV_NAME) != 0) + st_lsm6ds3_i2c_master_probe(cdata); + + device_init_wakeup(cdata->dev, true); + + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->indio_dev[n]); + + if (irq > 0) + st_lsm6ds3_deallocate_triggers(cdata); +deallocate_ring: + st_lsm6ds3_deallocate_rings(cdata); +free_fifo_data: + kfree(cdata->fifo_data); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_common_probe); + +void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq) +{ + int i; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_device_unregister(cdata->indio_dev[i]); + + if (irq > 0) + st_lsm6ds3_deallocate_triggers(cdata); + + st_lsm6ds3_deallocate_rings(cdata); + + kfree(cdata->fifo_data); + + if (strcmp(cdata->name, LSM6DS33_DEV_NAME) != 0) + st_lsm6ds3_i2c_master_exit(cdata); +} +EXPORT_SYMBOL(st_lsm6ds3_common_remove); + +#ifdef CONFIG_PM +int __maybe_unused st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata) +{ + int err, i; + u8 tmp_sensors_enabled; + struct lsm6ds3_sensor_data *sdata; + + tmp_sensors_enabled = cdata->sensors_enabled; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_MASK_ID_SIGN_MOTION) || (i == ST_MASK_ID_TILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + +#ifdef CONFIG_ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND + if ((BIT(i) & cdata->sensors_enabled) && + (i == ST_MASK_ID_STEP_COUNTER)) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_INT2_ADDR, + ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + continue; + } +#endif /* CONFIG_ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND */ + + err = st_lsm6ds3_set_enable(sdata, false, true); + if (err < 0) + return err; + } + cdata->sensors_enabled = tmp_sensors_enabled; + + if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + enable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_common_suspend); + +int __maybe_unused st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata) +{ + int err, i; + struct lsm6ds3_sensor_data *sdata; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_MASK_ID_SIGN_MOTION) || (i == ST_MASK_ID_TILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + + if (BIT(sdata->sindex) & cdata->sensors_enabled) { +#ifdef CONFIG_ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND + if (i == ST_MASK_ID_STEP_COUNTER) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_INT2_ADDR, + ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + continue; + } +#endif /* CONFIG_ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND */ + + err = st_lsm6ds3_set_enable(sdata, true, true); + if (err < 0) + return err; + } + } + + if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + disable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_common_resume); +#endif /* CONFIG_PM */ + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 core driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c.c b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c.c new file mode 100644 index 000000000000..655f7b051752 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c.c @@ -0,0 +1,182 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lsm6ds3.h" + +static int st_lsm6ds3_i2c_read(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int st_lsm6ds3_i2c_write(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + int err = 0; + u8 send[8]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, &msg, 1); + + return err; +} + +static const struct st_lsm6ds3_transfer_function st_lsm6ds3_tf_i2c = { + .write = st_lsm6ds3_i2c_write, + .read = st_lsm6ds3_i2c_read, +}; + +static int st_lsm6ds3_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct lsm6ds3_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + i2c_set_clientdata(client, cdata); + + cdata->spi_connection = false; + cdata->tf = &st_lsm6ds3_tf_i2c; + + err = st_lsm6ds3_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6ds3_i2c_remove(struct i2c_client *client) +{ + struct lsm6ds3_data *cdata = i2c_get_clientdata(client); + + st_lsm6ds3_common_remove(cdata, client->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_lsm6ds3_suspend(struct device *dev) +{ + struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6ds3_common_suspend(cdata); +} + +static int __maybe_unused st_lsm6ds3_resume(struct device *dev) +{ + struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6ds3_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6ds3_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume) +}; + +#define ST_LSM6DS3_PM_OPS (&st_lsm6ds3_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DS3_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id st_lsm6ds3_id_table[] = { + { LSM6DS3_DEV_NAME }, + { LSM6DS33_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6ds3_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id lsm6ds3_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = LSM6DS3_DEV_NAME, + }, + { + .compatible = "st,lsm6ds33", + .data = LSM6DS33_DEV_NAME, + }, + {} +}; +MODULE_DEVICE_TABLE(of, lsm6ds3_of_match); +#else /* CONFIG_OF */ +#define lsm6ds3_of_match NULL +#endif /* CONFIG_OF */ + +static struct i2c_driver st_lsm6ds3_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6ds3-i2c", + .pm = ST_LSM6DS3_PM_OPS, + .of_match_table = of_match_ptr(lsm6ds3_of_match), + }, + .probe = st_lsm6ds3_i2c_probe, + .remove = st_lsm6ds3_i2c_remove, + .id_table = st_lsm6ds3_id_table, +}; +module_i2c_driver(st_lsm6ds3_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c new file mode 100644 index 000000000000..2a2169e1b6fa --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c @@ -0,0 +1,1782 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3 i2c master driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_lsm6ds3.h" + +#define EXT0_INDEX 0 + +#define ST_LSM6DS3_ODR_LIST_NUM 4 +#define ST_LSM6DS3_SENSOR_HUB_OP_TIMEOUT 5 +#define ST_LSM6DS3_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DS3_EN_BIT 0x01 +#define ST_LSM6DS3_DIS_BIT 0x00 +#define ST_LSM6DS3_SLV0_ADDR_ADDR 0x02 +#define ST_LSM6DS3_SLV1_ADDR_ADDR 0x05 +#define ST_LSM6DS3_SLV2_ADDR_ADDR 0x08 +#define ST_LSM6DS3_SLV0_OUT_ADDR 0x2e +#define ST_LSM6DS3_INTER_PULLUP_ADDR 0x1a +#define ST_LSM6DS3_INTER_PULLUP_MASK 0x08 +#define ST_LSM6DS3_FUNC_MAX_RATE_ADDR 0x18 +#define ST_LSM6DS3_FUNC_MAX_RATE_MASK 0x02 +#define ST_LSM6DS3_DATAWRITE_SLV0 0x0e +#define ST_LSM6DS3_SLVX_READ 0x01 + +/* External sensors configuration */ +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6ds3_sensor_data *sdata); + +#define ST_LSM6DS3_EXT0_ADDR 0x1e +#define ST_LSM6DS3_EXT0_ADDR2 0x1c +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x3d +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x21 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x04 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x21 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x60 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x20 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x1c +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x04 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x05 +#define ST_LSM6DS3_EXT0_ODR2_HZ 40 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3_EXT0_ODR3_HZ 80 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x07 +#define ST_LSM6DS3_EXT0_PW_ADDR 0x22 +#define ST_LSM6DS3_EXT0_PW_MASK 0x03 +#define ST_LSM6DS3_EXT0_PW_OFF 0x02 +#define ST_LSM6DS3_EXT0_PW_ON 0x00 +#define ST_LSM6DS3_EXT0_GAIN_VALUE 438 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x28 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_BDU_ADDR 0x24 +#define ST_LSM6DS3_EXT0_BDU_MASK 0x40 +#define ST_LSM6DS3_EXT0_STD 0 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&lis3mdl_initialization) +#define ST_LSM6DS3_SELFTEST_EXT0_MIN 2281 +#define ST_LSM6DS3_SELFTEST_EXT0_MAX 6843 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN_Z 228 +#define ST_LSM6DS3_SELFTEST_EXT0_MAX_Z 2281 +#define ST_LSM6DS3_SELFTEST_ADDR1 0x20 +#define ST_LSM6DS3_SELFTEST_ADDR2 0x21 +#define ST_LSM6DS3_SELFTEST_ADDR3 0x22 +#define ST_LSM6DS3_SELFTEST_ADDR1_VALUE 0x1c +#define ST_LSM6DS3_SELFTEST_ADDR2_VALUE 0x40 +#define ST_LSM6DS3_SELFTEST_ADDR3_VALUE 0x00 +#define ST_LSM6DS3_SELFTEST_ENABLE 0x1d +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct lsm6ds3_sensor_data *sdata); + +#define ST_LSM6DS3_EXT0_ADDR 0x0c +#define ST_LSM6DS3_EXT0_ADDR2 0x0d +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x05 +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3_EXT0_PW_ADDR ST_LSM6DS3_EXT0_ODR_ADDR +#define ST_LSM6DS3_EXT0_PW_MASK ST_LSM6DS3_EXT0_ODR_MASK +#define ST_LSM6DS3_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3_EXT0_PW_ON ST_LSM6DS3_EXT0_ODR0_VALUE +#define ST_LSM6DS3_EXT0_GAIN_VALUE 6000 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3_EXT0_STD 0 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&akm09911_initialization) +#define ST_LSM6DS3_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN (-30) +#define ST_LSM6DS3_SELFTEST_EXT0_MAX 30 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN_Z (-400) +#define ST_LSM6DS3_SELFTEST_EXT0_MAX_Z (-50) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6ds3_sensor_data *sdata); + +#define ST_LSM6DS3_EXT0_ADDR 0x0c +#define ST_LSM6DS3_EXT0_ADDR2 0x0d +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x04 +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3_EXT0_PW_ADDR ST_LSM6DS3_EXT0_ODR_ADDR +#define ST_LSM6DS3_EXT0_PW_MASK ST_LSM6DS3_EXT0_ODR_MASK +#define ST_LSM6DS3_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3_EXT0_PW_ON ST_LSM6DS3_EXT0_ODR0_VALUE +#define ST_LSM6DS3_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3_EXT0_STD 0 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&akm09912_initialization) +#define ST_LSM6DS3_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN (-200) +#define ST_LSM6DS3_SELFTEST_EXT0_MAX 200 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN_Z (-1600) +#define ST_LSM6DS3_SELFTEST_EXT0_MAX_Z (-400) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09916 +#define ST_LSM6DS3_EXT0_ADDR 0x0c +#define ST_LSM6DS3_EXT0_ADDR2 0x0c +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x09 +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3_EXT0_PW_ADDR ST_LSM6DS3_EXT0_ODR_ADDR +#define ST_LSM6DS3_EXT0_PW_MASK ST_LSM6DS3_EXT0_ODR_MASK +#define ST_LSM6DS3_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3_EXT0_PW_ON ST_LSM6DS3_EXT0_ODR0_VALUE +#define ST_LSM6DS3_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3_EXT0_STD 0 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION NULL +#define ST_LSM6DS3_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN (-200) +#define ST_LSM6DS3_SELFTEST_EXT0_MAX 200 +#define ST_LSM6DS3_SELFTEST_EXT0_MIN_Z (-1000) +#define ST_LSM6DS3_SELFTEST_EXT0_MAX_Z (-200) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09916 */ + + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct lsm6ds3_sensor_data *sdata); + +#define ST_LSM6DS3_EXT0_ADDR 0x5d +#define ST_LSM6DS3_EXT0_ADDR2 0x5c +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DS3_EXT0_WAI_VALUE 0xb1 +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x11 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x80 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x10 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x70 +#define ST_LSM6DS3_EXT0_ODR0_HZ 1 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x01 +#define ST_LSM6DS3_EXT0_ODR1_HZ 10 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR2_HZ 25 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x03 +#define ST_LSM6DS3_EXT0_ODR3_HZ 50 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x04 +#define ST_LSM6DS3_EXT0_PW_ADDR ST_LSM6DS3_EXT0_ODR_ADDR +#define ST_LSM6DS3_EXT0_PW_MASK ST_LSM6DS3_EXT0_ODR_MASK +#define ST_LSM6DS3_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3_EXT0_PW_ON ST_LSM6DS3_EXT0_ODR0_VALUE +#define ST_LSM6DS3_EXT0_GAIN_VALUE 244 +#define ST_LSM6DS3_EXT0_OUT_P_L_ADDR 0x28 +#define ST_LSM6DS3_EXT0_OUT_T_L_ADDR 0x2b +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 5 +#define ST_LSM6DS3_EXT0_BDU_ADDR 0x10 +#define ST_LSM6DS3_EXT0_BDU_MASK 0x02 +#define ST_LSM6DS3_EXT0_STD 0 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&lps22hb_initialization) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS2MDL +static int lis2mdl_initialization(struct lsm6ds3_sensor_data *sdata); + +#define ST_LSM6DS3_EXT0_ADDR 0x1e +#define ST_LSM6DS3_EXT0_ADDR2 0x1e +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x4f +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x40 +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x60 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x20 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x60 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x0c +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x00 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x01 +#define ST_LSM6DS3_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x03 +#define ST_LSM6DS3_EXT0_PW_ADDR 0x60 +#define ST_LSM6DS3_EXT0_PW_MASK 0x03 +#define ST_LSM6DS3_EXT0_PW_OFF 0x02 +#define ST_LSM6DS3_EXT0_PW_ON 0x00 +#define ST_LSM6DS3_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x68 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x6a +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x6c +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_BDU_ADDR 0x62 +#define ST_LSM6DS3_EXT0_BDU_MASK 0x10 +#define ST_LSM6DS3_EXT0_STD 6 +#define ST_LSM6DS3_EXT0_TEMP_COMP_ADDR 0x60 +#define ST_LSM6DS3_EXT0_TEMP_COMP_MASK 0x80 +#define ST_LSM6DS3_EXT0_OFF_CANC_ADDR 0x61 +#define ST_LSM6DS3_EXT0_OFF_CANC_MASK 0x02 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&lis2mdl_initialization) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS2MDL */ + +/* SENSORS SUFFIX NAMES */ +#define ST_LSM6DS3_EXT0_SUFFIX_NAME "magn" +#define ST_LSM6DS3_EXT1_SUFFIX_NAME "press" + +#if defined(CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911) +#define ST_LSM6DS3_EXT0_HAS_SELFTEST 1 +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_MAGN */ + +#if defined(CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB) +#define ST_LSM6DS3_EXT0_HAS_FULLSCALE 1 +#endif + +#if defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911) +#define ST_LSM6DS3_EXT0_IS_AKM 1 +#define ST_LSM6DS3_SELFTEST_STATUS_REG 0x10 +#define ST_LSM6DS3_SELFTEST_ADDR 0x31 +#define ST_LSM6DS3_SELFTEST_ENABLE 0x10 +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM0099xx */ + + +struct st_lsm6ds3_i2c_master_odr_reg { + unsigned int hz; + u8 value; +}; + +struct st_lsm6ds3_i2c_master_odr_table { + u8 addr; + u8 mask; + struct st_lsm6ds3_i2c_master_odr_reg odr_avl[ST_LSM6DS3_ODR_LIST_NUM]; +}; + +static int st_lsm6ds3_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask); + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB +static const struct iio_chan_spec st_lsm6ds3_ext0_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_PRESSURE, 0, 0, IIO_NO_MOD, IIO_LE, + 24, 24, ST_LSM6DS3_EXT0_OUT_P_L_ADDR, 'u'), + ST_LSM6DS3_LSM_CHANNELS(IIO_TEMP, 0, 1, IIO_NO_MOD, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_T_L_ADDR, 's'), + ST_LSM6DS3_FLUSH_CHANNEL(IIO_PRESSURE), + IIO_CHAN_SOFT_TIMESTAMP(2) +}; +#else /* CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB */ +static const struct iio_chan_spec st_lsm6ds3_ext0_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_X_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_Z_L_ADDR, 's'), + ST_LSM6DS3_FLUSH_CHANNEL(IIO_MAGN), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB */ + +static int st_lsm6ds3_i2c_master_set_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr, bool force); + +static int st_lsm6ds3_i2c_master_write(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock); +static int st_lsm6ds3_i2c_master_read(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset); + +#ifdef ST_LSM6DS3_EXT0_HAS_SELFTEST +static ssize_t st_lsm6ds3_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_lsm6ds3_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_lsm6ds3_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +#endif /* ST_LSM6DS3_EXT0_HAS_SELFTEST */ + +static ssize_t st_lsm6ds3_i2c_master_sysfs_sampling_frequency_avail( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%d %d %d %d\n", 13, 26, 52, 104); +} + +static ssize_t st_lsm6ds3_i2c_master_sysfs_get_sampling_frequency( + struct device *dev, struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_lsm6ds3_i2c_master_sysfs_set_sampling_frequency( + struct device *dev, struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + mutex_lock(&sdata->cdata->odr_lock); + + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_lsm6ds3_i2c_master_set_odr(sdata, odr, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_lsm6ds3_i2c_master_sysfs_get_sampling_frequency, + st_lsm6ds3_i2c_master_sysfs_set_sampling_frequency); + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL( + st_lsm6ds3_i2c_master_sysfs_sampling_frequency_avail); + +static ST_LSM6DS3_HWFIFO_ENABLED(); +static ST_LSM6DS3_HWFIFO_WATERMARK(); +static ST_LSM6DS3_HWFIFO_WATERMARK_MIN(); +static ST_LSM6DS3_HWFIFO_WATERMARK_MAX(); +static ST_LSM6DS3_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6ds3_get_module_id, NULL, 0); + +#ifdef ST_LSM6DS3_EXT0_HAS_SELFTEST +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_lsm6ds3_i2c_master_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_lsm6ds3_i2c_master_sysfs_get_selftest_status, + st_lsm6ds3_i2c_master_sysfs_start_selftest, 0); +#endif /* ST_LSM6DS3_EXT0_HAS_SELFTEST */ + +static struct attribute *st_lsm6ds3_ext0_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef ST_LSM6DS3_EXT0_HAS_SELFTEST + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, +#endif /* ST_LSM6DS3_EXT0_HAS_SELFTEST */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3_ext0_attribute_group = { + .attrs = st_lsm6ds3_ext0_attributes, +}; + +static const struct iio_info st_lsm6ds3_ext0_info = { + .attrs = &st_lsm6ds3_ext0_attribute_group, + .read_raw = &st_lsm6ds3_i2c_master_read_raw, +}; + +struct st_lsm6ds3_iio_info_data { + char suffix_name[20]; + struct iio_info *info; + struct iio_chan_spec *channels; + int num_channels; +}; + +struct st_lsm6ds3_reg { + u8 addr; + u8 mask; + u8 def_value; +}; + +struct st_lsm6ds3_power_reg { + u8 addr; + u8 mask; + u8 off_value; + u8 on_value; + bool isodr; +}; + +struct st_lsm6ds3_custom_function { + int (*boot_initialization)(struct lsm6ds3_sensor_data *sdata); +}; + +static struct st_lsm6ds3_exs_list { + struct st_lsm6ds3_reg wai; + struct st_lsm6ds3_reg reset; + struct st_lsm6ds3_reg fullscale; + struct st_lsm6ds3_i2c_master_odr_table odr; + struct st_lsm6ds3_power_reg power; + u8 fullscale_value; + u8 samples_to_discard; + u8 read_data_len; + u8 num_data_channels; + bool available; + unsigned int gain; + u8 i2c_addr; + struct st_lsm6ds3_iio_info_data data; + struct st_lsm6ds3_custom_function cf; +} st_lsm6ds3_exs_list[] = { + { + .wai = { + .addr = ST_LSM6DS3_EXT0_WAI_ADDR, + .def_value = ST_LSM6DS3_EXT0_WAI_VALUE, + }, + .reset = { + .addr = ST_LSM6DS3_EXT0_RESET_ADDR, + .mask = ST_LSM6DS3_EXT0_RESET_MASK, + }, +#ifdef ST_LSM6DS3_EXT0_HAS_FULLSCALE + .fullscale = { + .addr = ST_LSM6DS3_EXT0_FULLSCALE_ADDR, + .mask = ST_LSM6DS3_EXT0_FULLSCALE_MASK, + .def_value = ST_LSM6DS3_EXT0_FULLSCALE_VALUE, + }, +#endif + .odr = { + .addr = ST_LSM6DS3_EXT0_ODR_ADDR, + .mask = ST_LSM6DS3_EXT0_ODR_MASK, + .odr_avl = { + { + .hz = ST_LSM6DS3_EXT0_ODR0_HZ, + .value = ST_LSM6DS3_EXT0_ODR0_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT0_ODR1_HZ, + .value = ST_LSM6DS3_EXT0_ODR1_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT0_ODR2_HZ, + .value = ST_LSM6DS3_EXT0_ODR2_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT0_ODR3_HZ, + .value = ST_LSM6DS3_EXT0_ODR3_VALUE, + }, + }, + }, + .power = { + .addr = ST_LSM6DS3_EXT0_PW_ADDR, + .mask = ST_LSM6DS3_EXT0_PW_MASK, + .off_value = ST_LSM6DS3_EXT0_PW_OFF, + .on_value = ST_LSM6DS3_EXT0_PW_ON, + }, + .samples_to_discard = ST_LSM6DS3_EXT0_STD, + .read_data_len = ST_LSM6DS3_EXT0_READ_DATA_LEN, + .num_data_channels = 3, + .available = false, + .gain = ST_LSM6DS3_EXT0_GAIN_VALUE, + .i2c_addr = ST_LSM6DS3_EXT0_ADDR, + .data = { + .suffix_name = ST_LSM6DS3_EXT0_SUFFIX_NAME, + .info = (struct iio_info *)&st_lsm6ds3_ext0_info, + .channels = (struct iio_chan_spec *)&st_lsm6ds3_ext0_ch, + .num_channels = ARRAY_SIZE(st_lsm6ds3_ext0_ch), + }, + .cf.boot_initialization = ST_LSM6DS3_EXT0_BOOT_FUNCTION, + } +}; + +static inline void st_lsm6ds3_master_wait_completed(struct lsm6ds3_data *cdata) +{ + msleep((1000U / cdata->trigger_odr) + 2); +} + +static int st_lsm6ds3_i2c_master_read(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset) +{ + int err; + u8 slave_conf[3]; + + slave_conf[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1) | + ST_LSM6DS3_SLVX_READ; + slave_conf[1] = reg_addr; + slave_conf[2] = (len & 0x07); + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + + if (en_sensor_hub) { + err = st_lsm6ds3_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + + st_lsm6ds3_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_LSM6DS3_SLV0_OUT_ADDR + + offset, len & 0x07, data, true); + if (err < 0) + goto i2c_master_read_unlock_mutex; + +#ifdef ST_LSM6DS3_EXT0_IS_AKM + if (read_status_end) { + slave_conf[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + slave_conf[1] = ST_LSM6DS3_EXT0_DATA_STATUS; + slave_conf[2] = 0x01; + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } +#endif /* ST_LSM6DS3_EXT0_IS_AKM */ + + if (en_sensor_hub) { + err = st_lsm6ds3_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + +i2c_master_read_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len & 0x07; +} + +static int st_lsm6ds3_i2c_master_write(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock) +{ + int err, i = 0; + u8 slave0_conf[2]; + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + while (i < len) { + slave0_conf[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1); + slave0_conf[1] = reg_addr + i; + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + slave0_conf[0] = data[i]; + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_DATAWRITE_SLV0, + slave0_conf, 1); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + if (en_sensor_hub) { + err = st_lsm6ds3_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + st_lsm6ds3_master_wait_completed(cdata); + + if (en_sensor_hub) { + err = st_lsm6ds3_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + i++; + } + + slave0_conf[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1); + slave0_conf[1] = st_lsm6ds3_exs_list[EXT0_INDEX].wai.addr; + + st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + +i2c_master_write_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len; +} + +static int st_lsm6ds3_i2c_master_write_data_with_mask( + struct lsm6ds3_data *cdata, u8 reg_addr, u8 mask, u8 data) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + mutex_lock(&cdata->i2c_transfer_lock); + disable_irq(cdata->irq); + + err = st_lsm6ds3_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + err = st_lsm6ds3_i2c_master_read(cdata, reg_addr, 1, + &old_data, false, false, true, + st_lsm6ds3_exs_list[0].read_data_len); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data != old_data) + err = st_lsm6ds3_i2c_master_write(cdata, reg_addr, + 1, &new_data, false, false); + + st_lsm6ds3_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + + return err; +} + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6ds3_sensor_data *sdata) +{ + + return st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata, + ST_LSM6DS3_EXT0_BDU_ADDR, + ST_LSM6DS3_EXT0_BDU_MASK, ST_LSM6DS3_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct lsm6ds3_sensor_data *sdata) +{ + int err; u8 data[ST_LSM6DS3_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6ds3_i2c_master_read(sdata->cdata, + ST_LSM6DS3_EXT0_SENSITIVITY_ADDR, + ST_LSM6DS3_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_lsm6ds3_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0]) * 1000) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1]) * 1000) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2]) * 1000) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6ds3_sensor_data *sdata) +{ + int err; u8 data[ST_LSM6DS3_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6ds3_i2c_master_read(sdata->cdata, + ST_LSM6DS3_EXT0_SENSITIVITY_ADDR, + ST_LSM6DS3_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_lsm6ds3_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0] - 128) * 500) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1] - 128) * 500) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2] - 128) * 500) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct lsm6ds3_sensor_data *sdata) +{ + + return st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata, + ST_LSM6DS3_EXT0_BDU_ADDR, + ST_LSM6DS3_EXT0_BDU_MASK, ST_LSM6DS3_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LPS22HB */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS2MDL +static int lis2mdl_initialization(struct lsm6ds3_sensor_data *sdata) +{ + int err; + + err = st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_EXT0_TEMP_COMP_ADDR, + ST_LSM6DS3_EXT0_TEMP_COMP_MASK, + 1); + if (err < 0) + return err; + + err = st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_EXT0_OFF_CANC_ADDR, + ST_LSM6DS3_EXT0_OFF_CANC_MASK, + 1); + if (err < 0) + return err; + + return st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_EXT0_BDU_ADDR, + ST_LSM6DS3_EXT0_BDU_MASK, + ST_LSM6DS3_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS2MDL */ + +#ifdef ST_LSM6DS3_EXT0_HAS_SELFTEST +static ssize_t st_lsm6ds3_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "absolute\n"); +} + +static ssize_t st_lsm6ds3_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + result = sdata->cdata->ext0_selftest_status; + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_LSM6DS3_SELFTEST_NA_MS; + else if (result < 0) + message = ST_LSM6DS3_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_LSM6DS3_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_lsm6ds3_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + int x_selftest = 0, y_selftest = 0, z_selftest = 0; + u8 outdata[8], reg_addr, reg_status = 0, temp_reg_status; +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL + int i, x = 0, y = 0, z = 0; + u8 reg_status2 = 0, reg_status3 = 0; + u8 reg_addr2, reg_addr3, temp_reg_status2, temp_reg_status3; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ +#ifdef ST_LSM6DS3_EXT0_IS_AKM + u8 temp, sh_config[3], timeout = 0; +#endif /* ST_LSM6DS3_EXT0_IS_AKM */ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + sdata->cdata->ext0_selftest_status = 0; + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + if (strncmp(buf, "absolute", size - 2) != 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = st_lsm6ds3_enable_sensor_hub(sdata->cdata, true, ST_MASK_ID_EXT0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL + reg_addr = ST_LSM6DS3_SELFTEST_ADDR1; + temp_reg_status = ST_LSM6DS3_SELFTEST_ADDR1_VALUE; + reg_addr2 = ST_LSM6DS3_SELFTEST_ADDR2; + temp_reg_status2 = ST_LSM6DS3_SELFTEST_ADDR2_VALUE; + reg_addr3 = ST_LSM6DS3_SELFTEST_ADDR3; + temp_reg_status3 = ST_LSM6DS3_SELFTEST_ADDR3_VALUE; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + +#ifdef ST_LSM6DS3_EXT0_IS_AKM + reg_addr = ST_LSM6DS3_SELFTEST_ADDR; + temp_reg_status = ST_LSM6DS3_SELFTEST_ENABLE; +#endif /* ST_LSM6DS3_EXT0_IS_AKM */ + + err = st_lsm6ds3_i2c_master_read(sdata->cdata, reg_addr, 1, + ®_status, false, true, false, + st_lsm6ds3_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + +#ifdef ST_LSM6DS3_EXT0_IS_AKM + /* SLAVE 1 is disabled for a while, dummy write to wai reg */ + sh_config[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6ds3_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; + + /* SLAVE 2 is disabled for a while, dummy read of wai reg */ + sh_config[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6ds3_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_SLV2_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; +#endif /* ST_LSM6DS3_EXT0_IS_AKM */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL + err = st_lsm6ds3_i2c_master_read(sdata->cdata, reg_addr2, 1, + ®_status2, false, true, false, + st_lsm6ds3_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + + err = st_lsm6ds3_i2c_master_read(sdata->cdata, reg_addr3, 1, + ®_status3, false, true, false, + st_lsm6ds3_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto disable_sensor_hub; + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr2, 1, + &temp_reg_status2, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr3, 1, + &temp_reg_status3, false, true); + if (err < 0) + goto restore_status_reg2; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_lsm6ds3_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 10; + y += ((s16)*(u16 *)&outdata[2]) / 10; + z += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + temp_reg_status = ST_LSM6DS3_SELFTEST_ENABLE; + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto restore_status_reg3; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_lsm6ds3_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 10; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 10; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); + if (err < 0) + goto restore_status_reg3; + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); + if (err < 0) + goto restore_status_reg2; + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6ds3_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + if ((abs(x_selftest - x) < ST_LSM6DS3_SELFTEST_EXT0_MIN) || + (abs(x_selftest - x) > ST_LSM6DS3_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(y_selftest - y) < ST_LSM6DS3_SELFTEST_EXT0_MIN) || + (abs(y_selftest - y) > ST_LSM6DS3_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(z_selftest - z) < ST_LSM6DS3_SELFTEST_EXT0_MIN_Z) || + (abs(z_selftest - z) > ST_LSM6DS3_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + +#ifdef ST_LSM6DS3_EXT0_IS_AKM + do { + msleep(1000U / sdata->cdata->trigger_odr); + + err = st_lsm6ds3_i2c_master_read(sdata->cdata, + ST_LSM6DS3_SELFTEST_STATUS_REG, 1, + &temp, false, true, false, 1); + if (err < 0) + goto restore_status_reg; + + timeout++; + } while (((temp & 0x01) == 0) && (timeout < 5)); + + if (timeout >= 5) { + err = -EINVAL; + goto restore_status_reg; + } + + err = st_lsm6ds3_i2c_master_read(sdata->cdata, + st_lsm6ds3_exs_list[0].data.channels[0].address, + st_lsm6ds3_exs_list[0].read_data_len, + outdata, false, true, true, 1); + if (err < 0) + goto restore_status_reg; + +#ifdef ST_LSM6DS3_EXT0_IS_AKM + /* SLAVE 2 recovering */ + sh_config[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6ds3_exs_list[0].data.channels[0].address; + sh_config[2] = st_lsm6ds3_exs_list[0].read_data_len; + + err = st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto restore_status_reg; +#endif /* ST_LSM6DS3_EXT0_IS_AKM */ + + err = st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6ds3_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + x_selftest = ((s16)*(u16 *)&outdata[0]); + y_selftest = ((s16)*(u16 *)&outdata[2]); + z_selftest = ((s16)*(u16 *)&outdata[4]); + +#if defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09911) + x_selftest *= sdata->c_gain[0]; + y_selftest *= sdata->c_gain[1]; + z_selftest *= sdata->c_gain[2]; + + x_selftest /= 10000; + y_selftest /= 10000; + z_selftest /= 10000; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM0991X */ + + if ((x_selftest < ST_LSM6DS3_SELFTEST_EXT0_MIN) || + (x_selftest > ST_LSM6DS3_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((y_selftest < ST_LSM6DS3_SELFTEST_EXT0_MIN) || + (y_selftest > ST_LSM6DS3_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((z_selftest < ST_LSM6DS3_SELFTEST_EXT0_MIN_Z) || + (z_selftest > ST_LSM6DS3_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* ST_LSM6DS3_EXT0_IS_AKM */ + + sdata->cdata->ext0_selftest_status = 1; + + mutex_unlock(&sdata->cdata->odr_lock); + + return size; + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL +restore_status_reg3: + st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); +restore_status_reg2: + st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ +restore_status_reg: + st_lsm6ds3_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); +disable_sensor_hub: + st_lsm6ds3_enable_sensor_hub(sdata->cdata, false, ST_MASK_ID_EXT0); + mutex_unlock(&sdata->cdata->odr_lock); + return err; +} +#endif /* ST_LSM6DS3_EXT0_HAS_SELFTEST */ + + +static int st_lsm6ds3_i2c_master_set_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr, bool force) +{ + int i, err, err2; + u8 value, mask, addr; + bool scan_odr = true; + unsigned int current_odr = sdata->cdata->v_odr[sdata->sindex]; + unsigned int current_hw_odr = sdata->cdata->hw_odr[sdata->sindex]; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + + if (scan_odr) { + switch (odr) { + case 13: + case 26: + case 52: + case 104: + break; + default: + return -EINVAL; + } + + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_exs_list[0].odr.odr_avl[i].hz >= odr) + break; + } + if (i == ST_LSM6DS3_ODR_LIST_NUM) + i--; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = odr; + return 0; + } + } + + addr = st_lsm6ds3_exs_list[0].odr.addr; + mask = st_lsm6ds3_exs_list[0].odr.mask; + value = st_lsm6ds3_exs_list[0].odr.odr_avl[i].value; + } else { + if (st_lsm6ds3_exs_list[0].power.isodr) { + addr = st_lsm6ds3_exs_list[0].power.addr; + mask = st_lsm6ds3_exs_list[0].power.mask; + value = st_lsm6ds3_exs_list[0].power.off_value; + } else + goto skip_i2c_write; + } + + sdata->cdata->samples_to_discard[ST_MASK_ID_EXT0] = + st_lsm6ds3_exs_list[0].samples_to_discard; + + err = st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + addr, mask, value); + if (err < 0) + return err; + +skip_i2c_write: + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = odr; + + if (!force) { + sdata->cdata->v_odr[sdata->sindex] = odr; + + err = st_lsm6ds3_enable_sensor_hub(sdata->cdata, + true, ST_MASK_ID_EXT0); + if (err < 0) { + sdata->cdata->hw_odr[sdata->sindex] = current_hw_odr; + sdata->cdata->v_odr[sdata->sindex] = current_odr; + do { + err2 = st_lsm6ds3_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + msleep(200); + } while (err2 < 0); + + return err; + } + } + + return 0; +} + +static int st_lsm6ds3_i2c_master_set_enable( + struct lsm6ds3_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + /* If odr != power this part should enable/disable sensor */ + if (!st_lsm6ds3_exs_list[0].power.isodr) { + if (enable) + reg_value = st_lsm6ds3_exs_list[0].power.on_value; + else + reg_value = st_lsm6ds3_exs_list[0].power.off_value; + + err = st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6ds3_exs_list[0].power.addr, + st_lsm6ds3_exs_list[0].power.mask, + reg_value); + if (err < 0) + return err; + } + + err = st_lsm6ds3_enable_sensor_hub(sdata->cdata, + enable, ST_MASK_ID_EXT0); + if (err < 0) + return err; + + err = st_lsm6ds3_i2c_master_set_odr(sdata, + enable ? sdata->cdata->v_odr[sdata->sindex] : 0, true); + if (err < 0) + goto disable_sensorhub; + + if (buffer) { + err = st_lsm6ds3_set_drdy_irq(sdata, enable); + if (err < 0) + goto restore_odr; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; + +restore_odr: + st_lsm6ds3_i2c_master_set_odr(sdata, + enable ? 0 : sdata->cdata->v_odr[sdata->sindex], true); +disable_sensorhub: + st_lsm6ds3_enable_sensor_hub(sdata->cdata, !enable, ST_MASK_ID_EXT0); + + return err; +} + +static int st_lsm6ds3_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + int err, ch_num_byte = ch->scan_type.storagebits >> 3; + u8 outdata[4]; + + if (ch_num_byte > ARRAY_SIZE(outdata)) + return -ENOMEM; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3_i2c_master_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + st_lsm6ds3_master_wait_completed(sdata->cdata); + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + ch_num_byte, outdata, true); + if (err < 0) { + st_lsm6ds3_i2c_master_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + err = st_lsm6ds3_i2c_master_set_enable(sdata, false, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (ch_num_byte > 2) + *val = (s32)get_unaligned_le32(outdata); + else + *val = (s16)get_unaligned_le16(outdata); + + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[ch->scan_index]; + + if (ch->type == IIO_TEMP) { + *val = 1; + *val2 = 0; + return IIO_VAL_INT; + } + + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6ds3_i2c_master_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) + return -EBUSY; +#endif /* CONFIG_ST_LSM6DS3_XL_DATA_INJECTION */ + + return 0; +} + +static int st_lsm6ds3_i2c_master_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) && + (indio_dev->buffer->length < 2 * ST_LSM6DS3_MAX_FIFO_LENGHT)) + return -EINVAL; + + sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); + if (!sdata->buffer_data) + return -ENOMEM; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3_i2c_master_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + return 0; +} + +static int st_lsm6ds3_i2c_master_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3_i2c_master_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + kfree(sdata->buffer_data); + + return err < 0 ? err : 0; +} + +static const struct iio_trigger_ops st_lsm6ds3_i2c_master_trigger_ops = { + .set_trigger_state = &st_lsm6ds3_trig_set_state, +}; + +int st_lsm6ds3_i2c_master_allocate_trigger(struct lsm6ds3_data *cdata) +{ + int err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->trig[ST_MASK_ID_EXT0]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + return -ENOMEM; + } + + iio_trigger_set_drvdata(cdata->trig[ST_MASK_ID_EXT0], + cdata->indio_dev[ST_MASK_ID_EXT0]); + cdata->trig[ST_MASK_ID_EXT0]->ops = &st_lsm6ds3_i2c_master_trigger_ops; + cdata->trig[ST_MASK_ID_EXT0]->dev.parent = cdata->dev; + + err = iio_trigger_register(cdata->trig[ST_MASK_ID_EXT0]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + goto deallocate_trigger; + } + + cdata->indio_dev[ST_MASK_ID_EXT0]->trig = cdata->trig[ST_MASK_ID_EXT0]; + + return 0; + +deallocate_trigger: + iio_trigger_free(cdata->trig[ST_MASK_ID_EXT0]); + return err; +} + +static void st_lsm6ds3_i2c_master_deallocate_trigger(struct lsm6ds3_data *cdata) +{ + iio_trigger_unregister(cdata->trig[ST_MASK_ID_EXT0]); +} + +static const struct iio_buffer_setup_ops st_lsm6ds3_i2c_master_buffer_setup_ops = { + .preenable = &st_lsm6ds3_i2c_master_buffer_preenable, + .postenable = &st_lsm6ds3_i2c_master_buffer_postenable, + .postdisable = &st_lsm6ds3_i2c_master_buffer_postdisable, +}; + +static inline irqreturn_t st_lsm6ds3_i2c_master_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +static int st_lsm6ds3_i2c_master_allocate_buffer(struct lsm6ds3_data *cdata) +{ + return iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_EXT0], + &st_lsm6ds3_i2c_master_handler_empty, NULL, + &st_lsm6ds3_i2c_master_buffer_setup_ops); +} + +static void st_lsm6ds3_i2c_master_deallocate_buffer(struct lsm6ds3_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_EXT0]); +} + +static int st_lsm6ds3_i2c_master_send_sensor_hub_parameters( + struct lsm6ds3_sensor_data *sdata) +{ + int err; + u8 sh_config[3]; + + /* SLAVE 0 is used by write */ + sh_config[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1); + sh_config[1] = st_lsm6ds3_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x01 | 0x20; + + err = st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + /* SLAVE 1 is used to read output data */ + sh_config[0] = (st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_LSM6DS3_EN_BIT; + sh_config[1] = st_lsm6ds3_exs_list[0].data.channels[0].address; + sh_config[2] = st_lsm6ds3_exs_list[0].read_data_len; + + err = st_lsm6ds3_write_embedded_registers(sdata->cdata, + ST_LSM6DS3_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6ds3_i2c_master_init_sensor(struct lsm6ds3_sensor_data *sdata) +{ + int err, ext_num = 0; + + err = st_lsm6ds3_i2c_master_send_sensor_hub_parameters(sdata); + if (err < 0) + return err; + + sdata->c_gain[0] = st_lsm6ds3_exs_list[ext_num].gain; + sdata->c_gain[1] = st_lsm6ds3_exs_list[ext_num].gain; + sdata->c_gain[2] = st_lsm6ds3_exs_list[ext_num].gain; + + if ((st_lsm6ds3_exs_list[ext_num].power.addr == + st_lsm6ds3_exs_list[ext_num].odr.addr) && + (st_lsm6ds3_exs_list[ext_num].power.mask == + st_lsm6ds3_exs_list[ext_num].odr.mask)) + st_lsm6ds3_exs_list[ext_num].power.isodr = true; + else + st_lsm6ds3_exs_list[ext_num].power.isodr = false; + + err = st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6ds3_exs_list[ext_num].reset.addr, + st_lsm6ds3_exs_list[ext_num].reset.mask, + ST_LSM6DS3_EN_BIT); + if (err < 0) + return err; + + usleep_range(200, 1000); + + if (st_lsm6ds3_exs_list[ext_num].fullscale.addr > 0) { + err = st_lsm6ds3_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6ds3_exs_list[ext_num].fullscale.addr, + st_lsm6ds3_exs_list[ext_num].fullscale.mask, + st_lsm6ds3_exs_list[ext_num].fullscale.def_value); + if (err < 0) + return err; + } + + if (st_lsm6ds3_exs_list[0].cf.boot_initialization != NULL) { + err = st_lsm6ds3_exs_list[0].cf.boot_initialization(sdata); + if (err < 0) + return err; + } + + err = st_lsm6ds3_i2c_master_set_enable(sdata, false, false); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6ds3_i2c_master_allocate_device(struct lsm6ds3_data *cdata) +{ + int err; + struct lsm6ds3_sensor_data *sdata_ext; + + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + + sdata_ext->num_data_channels = + st_lsm6ds3_exs_list[0].num_data_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->name = kasprintf(GFP_KERNEL, + "%s_%s", cdata->name, + st_lsm6ds3_exs_list[0].data.suffix_name); + + cdata->indio_dev[ST_MASK_ID_EXT0]->info = + st_lsm6ds3_exs_list[0].data.info; + cdata->indio_dev[ST_MASK_ID_EXT0]->channels = + st_lsm6ds3_exs_list[0].data.channels; + cdata->indio_dev[ST_MASK_ID_EXT0]->num_channels = + st_lsm6ds3_exs_list[0].data.num_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->modes = INDIO_DIRECT_MODE; + + sdata_ext->data_out_reg = ST_LSM6DS3_SLV0_OUT_ADDR; + + err = st_lsm6ds3_i2c_master_init_sensor(sdata_ext); + if (err < 0) + return err; + + err = st_lsm6ds3_i2c_master_allocate_buffer(cdata); + if (err < 0) + return err; + + err = st_lsm6ds3_i2c_master_allocate_trigger(cdata); + if (err < 0) + goto iio_deallocate_buffer; + + err = iio_device_register(cdata->indio_dev[ST_MASK_ID_EXT0]); + if (err < 0) + goto iio_deallocate_trigger; + + return 0; + +iio_deallocate_trigger: + st_lsm6ds3_i2c_master_deallocate_trigger(cdata); +iio_deallocate_buffer: + st_lsm6ds3_i2c_master_deallocate_buffer(cdata); + + return err; +} + +static void st_lsm6ds3_i2c_master_deallocate_device(struct lsm6ds3_data *cdata) +{ + iio_device_unregister(cdata->indio_dev[ST_MASK_ID_EXT0]); + st_lsm6ds3_i2c_master_deallocate_trigger(cdata); + st_lsm6ds3_i2c_master_deallocate_buffer(cdata); +} + +int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata) +{ + int err, i; + u8 sh_config[3]; + u8 wai, i2c_address; + struct lsm6ds3_sensor_data *sdata_ext; + + mutex_init(&cdata->i2c_transfer_lock); + cdata->ext0_available = false; + cdata->ext0_selftest_status = false; + +#ifdef CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_INTER_PULLUP_ADDR, + ST_LSM6DS3_INTER_PULLUP_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; +#endif /* CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP */ + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_MAX_RATE_ADDR, + ST_LSM6DS3_FUNC_MAX_RATE_MASK, 1, true); + if (err < 0) + return err; + + cdata->indio_dev[ST_MASK_ID_EXT0] = devm_iio_device_alloc(cdata->dev, + sizeof(*sdata_ext)); + if (!cdata->indio_dev[ST_MASK_ID_EXT0]) + return -ENOMEM; + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + sdata_ext->cdata = cdata; + sdata_ext->sindex = ST_MASK_ID_EXT0; + cdata->samples_to_discard_2[ST_MASK_ID_EXT0] = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].sip = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = 0; + + for (i = 0; i < 2; i++) { + if (i == 0) + i2c_address = ST_LSM6DS3_EXT0_ADDR; + else + i2c_address = ST_LSM6DS3_EXT0_ADDR2; + + /* to check if sensor is available use SLAVE0 first time */ + sh_config[0] = (i2c_address << 1) | 0x01; + sh_config[1] = st_lsm6ds3_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x01; + + err = st_lsm6ds3_write_embedded_registers(cdata, + ST_LSM6DS3_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + err = st_lsm6ds3_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + msleep(100); + + st_lsm6ds3_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_LSM6DS3_SLV0_OUT_ADDR, + 1, &wai, true); + if (err < 0) { + err = st_lsm6ds3_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + continue; + } + + err = st_lsm6ds3_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + st_lsm6ds3_exs_list[EXT0_INDEX].i2c_addr = i2c_address; + break; + } + if (i == 2) + goto ext0_sensor_not_available; + + /* after wai check SLAVE0 is used for write, SLAVE1 for async read + and SLAVE2 to read sensor output data */ + + if (wai != st_lsm6ds3_exs_list[EXT0_INDEX].wai.def_value) { + dev_err(cdata->dev, "wai value of external sensor 0 mismatch\n"); + return err; + } + + err = st_lsm6ds3_i2c_master_allocate_device(cdata); + if (err < 0) + return err; + + cdata->ext0_available = true; + + return 0; + +ext0_sensor_not_available: + dev_err(cdata->dev, "external sensor 0 not available\n"); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_i2c_master_probe); + +int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata) +{ + if (cdata->ext0_available) + st_lsm6ds3_i2c_master_deallocate_device(cdata); + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_i2c_master_exit); diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_spi.c b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_spi.c new file mode 100644 index 000000000000..ecc9d263d471 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_spi.c @@ -0,0 +1,200 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lsm6ds3.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lsm6ds3_spi_read(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int st_lsm6ds3_spi_write(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_LSM6DS3_RX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static const struct st_lsm6ds3_transfer_function st_lsm6ds3_tf_spi = { + .write = st_lsm6ds3_spi_write, + .read = st_lsm6ds3_spi_read, +}; + +static int st_lsm6ds3_spi_probe(struct spi_device *spi) +{ + int err; + struct lsm6ds3_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + spi_set_drvdata(spi, cdata); + + cdata->spi_connection = true; + cdata->tf = &st_lsm6ds3_tf_spi; + + err = st_lsm6ds3_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6ds3_spi_remove(struct spi_device *spi) +{ + struct lsm6ds3_data *cdata = spi_get_drvdata(spi); + + st_lsm6ds3_common_remove(cdata, spi->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_lsm6ds3_suspend(struct device *dev) +{ + struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6ds3_common_suspend(cdata); +} + +static int __maybe_unused st_lsm6ds3_resume(struct device *dev) +{ + struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6ds3_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6ds3_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume) +}; + +#define ST_LSM6DS3_PM_OPS (&st_lsm6ds3_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DS3_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id st_lsm6ds3_id_table[] = { + { LSM6DS3_DEV_NAME }, + { LSM6DS33_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6ds3_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id lsm6ds3_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = LSM6DS3_DEV_NAME, + }, + { + .compatible = "st,lsm6ds33", + .data = LSM6DS33_DEV_NAME, + }, + {} +}; +MODULE_DEVICE_TABLE(of, lsm6ds3_of_match); +#else /* CONFIG_OF */ +#define lsm6ds3_of_match NULL +#endif /* CONFIG_OF */ + +static struct spi_driver st_lsm6ds3_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6ds3-spi", + .pm = ST_LSM6DS3_PM_OPS, + .of_match_table = of_match_ptr(lsm6ds3_of_match), + }, + .probe = st_lsm6ds3_spi_probe, + .remove = st_lsm6ds3_spi_remove, + .id_table = st_lsm6ds3_id_table, +}; +module_spi_driver(st_lsm6ds3_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_trigger.c new file mode 100644 index 000000000000..b88272659b27 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_trigger.c @@ -0,0 +1,210 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3 trigger driver + * + * MEMS Software Solutions Team + * + * Copyright 2014-2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6ds3.h" + +#define ST_LSM6DS3_DIS_BIT 0x00 +#define ST_LSM6DS3_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b +#define ST_LSM6DS3_ACCEL_DATA_AVL_ADDR 0x1e + +#define ST_LSM6DS3_ACCEL_DATA_AVL 0x01 +#define ST_LSM6DS3_GYRO_DATA_AVL 0x02 +#define ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10 +#define ST_LSM6DS3_SRC_TILT_DATA_AVL 0x20 +#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80 +#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_OVR 0x08 +#define ST_LSM6DS3_FIFO_DATA_AVL 0x80 +#define ST_LSM6DS3_FIFO_DATA_OVR 0x40 + + +static irqreturn_t lsm6ds3_irq_management(int irq, void *private) +{ + int err; + bool push; + bool force_read_accel = false; + struct lsm6ds3_data *cdata = private; + u8 src_accel_gyro = 0, src_dig_func = 0; + + cdata->timestamp = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + (BIT(ST_MASK_ID_ACCEL) | BIT(ST_MASK_ID_GYRO) | + BIT(ST_MASK_ID_EXT0))) { + err = cdata->tf->read(cdata, ST_LSM6DS3_ACCEL_DATA_AVL_ADDR, + 1, &src_accel_gyro, true); + if (err < 0) + goto read_fifo_status; + + if (src_accel_gyro & ST_LSM6DS3_ACCEL_DATA_AVL) { +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) + & BIT(ST_MASK_ID_EXT0)) { + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples++; + force_read_accel = true; + + if ((cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = 0; + } else { + push = false; + } + + lsm6ds3_read_output_data(cdata, ST_MASK_ID_EXT0, push); + } +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + BIT(ST_MASK_ID_ACCEL)) { + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples++; + + if ((cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = 0; + } else { + push = false; + } + + lsm6ds3_read_output_data(cdata, ST_MASK_ID_ACCEL, push); + } else { + if (force_read_accel) + lsm6ds3_read_output_data(cdata, ST_MASK_ID_ACCEL, false); + } + + } + + if (src_accel_gyro & ST_LSM6DS3_GYRO_DATA_AVL) { + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & BIT(ST_MASK_ID_GYRO)) + lsm6ds3_read_output_data(cdata, ST_MASK_ID_GYRO, true); + } + } + +read_fifo_status: + if (cdata->sensors_use_fifo) + st_lsm6ds3_read_fifo(cdata, false); + + err = cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, + 1, &src_dig_func, true); + if (err < 0) + goto exit_irq; + + if (src_dig_func & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) { + if (cdata->sensors_enabled & BIT(ST_MASK_ID_STEP_DETECTOR)) { + st_lsm6ds3_push_data_with_timestamp(cdata, + ST_MASK_ID_STEP_DETECTOR, NULL, cdata->timestamp); + } + + if ((cdata->sign_motion_event_ready) && + (cdata->sensors_enabled & + BIT(ST_MASK_ID_SIGN_MOTION))) { + iio_push_event(cdata->indio_dev[ + ST_MASK_ID_SIGN_MOTION], + IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->timestamp); + + cdata->sign_motion_event_ready = false; + } + } + + if (src_dig_func & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_OVR) + cdata->num_steps += (1 << 16); + + if (src_dig_func & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL) + iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_STEP_COUNTER]); + + if ((src_dig_func & ST_LSM6DS3_SRC_TILT_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_TILT))) { + st_lsm6ds3_push_data_with_timestamp(cdata, + ST_MASK_ID_TILT, NULL, cdata->timestamp); + } + +exit_irq: + return IRQ_HANDLED; +} + +int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[i] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + if (!cdata->trig[i]) { + dev_err(cdata->dev, + "failed to allocate iio trigger.\n"); + err = -ENOMEM; + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]); + cdata->trig[i]->ops = trigger_ops; + cdata->trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, NULL, lsm6ds3_irq_management, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_trigger_register(cdata->trig[n]); + if (err < 0) { + dev_err(cdata->dev, + "failed to register iio trigger.\n"); + goto free_irq; + } + cdata->indio_dev[n]->trig = cdata->trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->trig[i]); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_allocate_triggers); + +void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_trigger_unregister(cdata->trig[i]); +} +EXPORT_SYMBOL(st_lsm6ds3_deallocate_triggers); diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/Kconfig b/drivers/iio/stm/imu/st_lsm6ds3h/Kconfig new file mode 100644 index 000000000000..0acac5481a33 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/Kconfig @@ -0,0 +1,93 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# st-lsm6ds3h drivers for STMicroelectronics combo sensor +# + +menuconfig ST_LSM6DS3H_IIO + tristate "STMicroelectronics LSM6DS3H sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select ST_LSM6DS3H_I2C_IIO if (I2C) + select ST_LSM6DS3H_SPI_IIO if (SPI) + help + This driver supports LSM6DS3H sensor. + It is a gyroscope/accelerometer combo device. + This driver can be built as a module. The module will be called + st-lsm6ds3h. + +if ST_LSM6DS3H_IIO + +config ST_LSM6DS3H_I2C_IIO + tristate + depends on ST_LSM6DS3H_IIO + depends on I2C + +config ST_LSM6DS3H_SPI_IIO + tristate + depends on ST_LSM6DS3H_IIO + depends on SPI + +config ST_LSM6DS3H_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on ST_LSM6DS3H_IIO + range 0 4096 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +config ST_LSM6DS3H_STEP_COUNTER_ON_DURING_SUSPEND + bool "Keep Step counter on during suspend" + depends on ST_LSM6DS3H_IIO + default n + help + During suspend step counter is kept on if enabled. Only interrupt + is disabled. + +menuconfig ST_LSM6DS3H_IIO_MASTER_SUPPORT + bool "I2C master controller" + depends on I2C && ST_LSM6DS3H_IIO + default n + help + Added support for I2C master controller. Only one slave sensor is + supported. + +if ST_LSM6DS3H_IIO_MASTER_SUPPORT + +config ST_LSM6DS3H_ENABLE_INTERNAL_PULLUP + bool "Enabled internals pull-up resistors" + default y + +choice + prompt "External sensor 0" + default ST_LSM6DS3H_IIO_EXT0_LIS3MDL + help + Choose the external sensor 0 connected to LSM6DS3. + +config ST_LSM6DS3H_IIO_EXT0_LIS3MDL + bool "LIS3MDL" +config ST_LSM6DS3H_IIO_EXT0_AKM09911 + bool "AKM09911" +config ST_LSM6DS3H_IIO_EXT0_AKM09912 + bool "AKM09912" +config ST_LSM6DS3H_IIO_EXT0_AKM09916 + bool "AKM09916" +config ST_LSM6DS3H_IIO_EXT0_LPS22HB + bool "LPS22HB" +endchoice + +endif + +config ST_LSM6DS3H_XL_DATA_INJECTION + bool "Enable XL data injection support" + depends on ST_LSM6DS3H_IIO + default n + help + This option enables the accelerometer data injection + support. The device functions may so use an injected + pattern instead of taking the real sensor data. + +endif #ST_LSM6DS3H_IIO diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/Makefile b/drivers/iio/stm/imu/st_lsm6ds3h/Makefile new file mode 100644 index 000000000000..a45359cbe352 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/Makefile @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Makefile for STMicroelectronics LSM6DS3H sensor. +# + +obj-$(CONFIG_ST_LSM6DS3H_IIO) += st_lsm6ds3h.o +st_lsm6ds3h-objs := st_lsm6ds3h_core.o +obj-$(CONFIG_ST_LSM6DS3H_I2C_IIO) += st_lsm6ds3h_i2c.o +obj-$(CONFIG_ST_LSM6DS3H_SPI_IIO) += st_lsm6ds3h_spi.o + +st_lsm6ds3h-$(CONFIG_IIO_BUFFER) += st_lsm6ds3h_buffer.o +st_lsm6ds3h-$(CONFIG_IIO_TRIGGER) += st_lsm6ds3h_trigger.o +st_lsm6ds3h-$(CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT) += st_lsm6ds3h_i2c_master.o diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h.h b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h.h new file mode 100644 index 000000000000..0b1cc0f503c3 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h.h @@ -0,0 +1,370 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lsm6ds3h driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DS3H_H +#define ST_LSM6DS3H_H + +#include +#include + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT +#include +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + +#define LSM6DS3H_DEV_NAME "lsm6ds3h" + +enum st_mask_id { + ST_MASK_ID_ACCEL = 0, + ST_MASK_ID_GYRO, + ST_MASK_ID_SIGN_MOTION, + ST_MASK_ID_STEP_COUNTER, + ST_MASK_ID_STEP_DETECTOR, + ST_MASK_ID_TILT, + ST_MASK_ID_EXT0, + ST_MASK_ID_HW_PEDOMETER, + ST_MASK_ID_SENSOR_HUB, + ST_MASK_ID_DIGITAL_FUNC, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP, +}; + +#define ST_INDIO_DEV_NUM 6 + +#define ST_LSM6DS3H_TX_MAX_LENGTH 12 +#define ST_LSM6DS3H_RX_MAX_LENGTH 4097 + +#define ST_LSM6DS3H_BYTE_FOR_CHANNEL 2 +#define ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE 6 + +#define ST_LSM6DS3H_MAX_FIFO_SIZE 4096 +#define ST_LSM6DS3H_MAX_FIFO_THRESHOLD 546 +#define ST_LSM6DS3H_MAX_FIFO_LENGHT (ST_LSM6DS3H_MAX_FIFO_SIZE / \ + ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE) + +#define ST_LSM6DS3H_SELFTEST_NA_MS "na" +#define ST_LSM6DS3H_SELFTEST_FAIL_MS "fail" +#define ST_LSM6DS3H_SELFTEST_PASS_MS "pass" + +#define ST_LSM6DS3H_WAKE_UP_SENSORS (BIT(ST_MASK_ID_SIGN_MOTION) | \ + BIT(ST_MASK_ID_TILT)) + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT +#define ST_LSM6DS3H_NUM_CLIENTS 1 +#else /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ +#define ST_LSM6DS3H_NUM_CLIENTS 0 +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + +#define ST_LSM6DS3H_LSM_CHANNELS(device_type, modif, index, mod, \ + endian, sbits, rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +extern const struct iio_event_spec lsm6ds3h_fifo_flush_event; + +#define ST_LSM6DS3H_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &lsm6ds3h_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_LSM6DS3H_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + st_lsm6ds3h_sysfs_get_hwfifo_enabled,\ + st_lsm6ds3h_sysfs_set_hwfifo_enabled, 0); + +#define ST_LSM6DS3H_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + st_lsm6ds3h_sysfs_get_hwfifo_watermark,\ + st_lsm6ds3h_sysfs_set_hwfifo_watermark, 0); + +#define ST_LSM6DS3H_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + st_lsm6ds3h_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_LSM6DS3H_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + st_lsm6ds3h_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_LSM6DS3H_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + st_lsm6ds3h_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +struct st_lsm6ds3h_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[ST_LSM6DS3H_RX_MAX_LENGTH]; + u8 tx_buf[ST_LSM6DS3H_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct lsm6ds3h_out_decimation { + short decimator; + short num_samples; +}; + +struct lsm6ds3h_fifo_output { + u8 sip; + int64_t deltatime; + int64_t deltatime_default; + int64_t timestamp; + int64_t timestamp_p; + short decimator; + short num_samples; + bool initialized; +}; + +/* struct lsm6ds3h_data - common data for i2c or spi driver instance + * @name: pointer to the device name (i2c name or spi modalias). + * @enable_digfunc_mask: mask used to enable/disable hw digital functions. + * @enable_pedometer_mask: mask used to enable/disable hw pedometer function. + * @enable_sensorhub_mask: mask used to enable/disable sensor-hub feature. + * @irq_enable_fifo_mask: mask used to enable/disable fifo irq. + * @irq_enable_accel_ext_mask: mask used to enable/disable accel irq. + * @hw_odr: physical sensor odr expressed in Hz. + * @v_odr: requested sensor odr by userspace expressed in Hz. + * @hwfifo_enabled: is hwfifo enabled? + * @hwfifo_decimator: hwfifo decimator factor. + * @hwfifo_watermark: hwfifo watermark value. + * @samples_to_discard: samples to discard due to ODR switch. + * @nofifo_decimation: output status when fifo is disabled. + * @fifo_output: output status when fifo is enabled. + * @sensors_enabled: sensors enabled mask. + * @sensors_use_fifo: sensors use fifo mask. + * @accel_odr_dependency: odr dependency: accel, sensor-hub, dig-func. + * @accel_on: accel is going to be enabled during fifo odr switch? + * @magn_on: magn is going to be enabled during fifo odr switch? + * @odr_lock: mutex to avoid race condition during odr switch. + * @reset_steps: do I need to reset number of steps? + * @fifo_data: fifo data. + * @gyro_selftest_status: gyroscope selftest result. + * @accel_selftest_status: accelerometer selftest result. + * @irq: irq number. + * @timestamp: timestamp value from boot process. + * @module_id: identify iio devices of the same sensor module. + */ +struct lsm6ds3h_data { + const char *name; + + u16 enable_digfunc_mask; + u16 enable_pedometer_mask; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + u16 enable_sensorhub_mask; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + u16 irq_enable_fifo_mask; + u16 irq_enable_accel_ext_mask; + + unsigned int hw_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int trigger_odr; + + bool hwfifo_enabled[ST_INDIO_DEV_NUM + 1]; + u8 hwfifo_decimator[ST_INDIO_DEV_NUM + 1]; + u16 hwfifo_watermark[ST_INDIO_DEV_NUM + 1]; + u16 fifo_watermark; + + u8 samples_to_discard[ST_INDIO_DEV_NUM + 1]; + u8 samples_to_discard_2[ST_INDIO_DEV_NUM + 1]; + struct lsm6ds3h_out_decimation nofifo_decimation[ST_INDIO_DEV_NUM + 1]; + struct lsm6ds3h_fifo_output fifo_output[ST_INDIO_DEV_NUM + 1]; + + u16 sensors_enabled; + u16 sensors_use_fifo; + u64 num_steps; + + int accel_odr_dependency[3]; + + bool accel_on; + bool magn_on; + enum fifo_mode fifo_status; + + struct mutex odr_lock; + + bool reset_steps; + + u8 *fifo_data; + u8 accel_last_push[6]; + u8 gyro_last_push[6]; + u8 ext0_last_push[6]; + int8_t gyro_selftest_status; + int8_t accel_selftest_status; + + int irq; + + s64 timestamp; + int64_t fifo_enable_timestamp; + int64_t slower_counter; + uint8_t slower_id; + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + bool injection_mode; + s64 last_injection_timestamp; + struct hrtimer injection_timer; + struct work_struct injection_work; + spinlock_t injection_spinlock; + u8 injection_data[30]; + u8 injection_samples; +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + struct work_struct data_work; + + struct device *dev; + struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + 1]; + struct iio_trigger *trig[ST_INDIO_DEV_NUM + 1]; + struct mutex bank_registers_lock; + struct mutex fifo_lock; + u32 module_id; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + bool ext0_available; + int8_t ext0_selftest_status; + struct mutex i2c_transfer_lock; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + const struct st_lsm6ds3h_transfer_function *tf; + struct st_lsm6ds3h_transfer_buffer tb; +}; + +struct st_lsm6ds3h_transfer_function { + int (*write)(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); + int (*read)(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); +}; + +struct lsm6ds3h_sensor_data { + struct lsm6ds3h_data *cdata; + + unsigned int c_gain[3]; + + u8 num_data_channels; + u8 sindex; + u8 data_out_reg; + u8 *buffer_data; +}; + +int st_lsm6ds3h_write_data_with_mask(struct lsm6ds3h_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock); + +int st_lsm6ds3h_push_data_with_timestamp(struct lsm6ds3h_data *cdata, + u8 index, u8 *data, int64_t timestamp); + +int st_lsm6ds3h_common_probe(struct lsm6ds3h_data *cdata, int irq); +void st_lsm6ds3h_common_remove(struct lsm6ds3h_data *cdata, int irq); + +int st_lsm6ds3h_set_enable(struct lsm6ds3h_sensor_data *sdata, bool enable, bool buffer); +int st_lsm6ds3h_set_fifo_mode(struct lsm6ds3h_data *cdata, enum fifo_mode fm); +int st_lsm6ds3h_enable_sensor_hub(struct lsm6ds3h_data *cdata, bool enable, + enum st_mask_id id); +int lsm6ds3h_read_output_data(struct lsm6ds3h_data *cdata, int sindex, bool push); +int st_lsm6ds3h_set_drdy_irq(struct lsm6ds3h_sensor_data *sdata, bool state); + +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3h_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3h_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3h_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6ds3h_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +#ifdef CONFIG_IIO_BUFFER +int st_lsm6ds3h_allocate_rings(struct lsm6ds3h_data *cdata); +void st_lsm6ds3h_deallocate_rings(struct lsm6ds3h_data *cdata); +int st_lsm6ds3h_trig_set_state(struct iio_trigger *trig, bool state); +int st_lsm6ds3h_read_fifo(struct lsm6ds3h_data *cdata, bool async); +#define ST_LSM6DS3H_TRIGGER_SET_STATE (&st_lsm6ds3h_trig_set_state) +#else /* CONFIG_IIO_BUFFER */ +static inline int st_lsm6ds3h_allocate_rings(struct lsm6ds3h_data *cdata) +{ + return 0; +} +static inline void st_lsm6ds3h_deallocate_rings(struct lsm6ds3h_data *cdata) +{ +} +static inline int st_lsm6ds3h_read_fifo(struct lsm6ds3h_data *cdata, bool async) +{ + return 0; +} +#define ST_LSM6DS3H_TRIGGER_SET_STATE NULL +#endif /* CONFIG_IIO_BUFFER */ + +#ifdef CONFIG_IIO_TRIGGER +int st_lsm6ds3h_allocate_triggers(struct lsm6ds3h_data *cdata, + const struct iio_trigger_ops *trigger_ops); +void st_lsm6ds3h_deallocate_triggers(struct lsm6ds3h_data *cdata); +void st_lsm6ds3h_flush_works(void); +#else /* CONFIG_IIO_TRIGGER */ +static inline int st_lsm6ds3h_allocate_triggers(struct lsm6ds3h_data *cdata, + const struct iio_trigger_ops *trigger_ops, int irq) +{ + return 0; +} +static inline void st_lsm6ds3h_deallocate_triggers(struct lsm6ds3h_data *cdata, + int irq) +{ + return; +} +static inline void st_lsm6ds3h_flush_works(void) +{ + return; +} +#endif /* CONFIG_IIO_TRIGGER */ + +#ifdef CONFIG_PM +int st_lsm6ds3h_common_suspend(struct lsm6ds3h_data *cdata); +int st_lsm6ds3h_common_resume(struct lsm6ds3h_data *cdata); +#endif /* CONFIG_PM */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT +int st_lsm6ds3h_write_embedded_registers(struct lsm6ds3h_data *cdata, + u8 reg_addr, u8 *data, int len); +int st_lsm6ds3h_i2c_master_probe(struct lsm6ds3h_data *cdata); +int st_lsm6ds3h_i2c_master_exit(struct lsm6ds3h_data *cdata); +#else /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ +static inline int st_lsm6ds3h_i2c_master_probe(struct lsm6ds3h_data *cdata) +{ + return 0; +} +static inline int st_lsm6ds3h_i2c_master_exit(struct lsm6ds3h_data *cdata) +{ + return 0; +} +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + +#endif /* ST_LSM6DS3H_H */ diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_buffer.c b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_buffer.c new file mode 100644 index 000000000000..5e6e49e11685 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_buffer.c @@ -0,0 +1,644 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3h buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_lsm6ds3h.h" + +#define ST_LSM6DS3H_ENABLE_AXIS 0x07 +#define ST_LSM6DS3H_FIFO_DIFF_L 0x3a +#define ST_LSM6DS3H_FIFO_DIFF_MASK 0x0f +#define ST_LSM6DS3H_FIFO_DATA_OUT_L 0x3e +#define ST_LSM6DS3H_FIFO_DATA_OVR 0x40 +#define ST_LSM6DS3H_FIFO_DATA_EMPTY 0x10 +#define ST_LSM6DS3H_STEP_MASK_64BIT (0xFFFFFFFFFFFF0000) + +#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \ + (d) : (c)) : ((b == 0) ? \ + (c) : (d))) + +int st_lsm6ds3h_push_data_with_timestamp(struct lsm6ds3h_data *cdata, + u8 index, u8 *data, int64_t timestamp) +{ + size_t offset; + int i, n = 0; + struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels; + uint16_t bfch, bfchs_out = 0, bfchs_in = 0; + struct lsm6ds3h_sensor_data *sdata = iio_priv(cdata->indio_dev[index]); + + if (timestamp <= cdata->fifo_output[index].timestamp_p) + return -EINVAL; + + for (i = 0; i < sdata->num_data_channels; i++) { + bfch = chs[i].scan_type.storagebits >> 3; + + if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) { + memcpy(&sdata->buffer_data[bfchs_out], + &data[bfchs_in], bfch); + n++; + bfchs_out += bfch; + } + + bfchs_in += bfch; + } + + if (cdata->indio_dev[index]->scan_timestamp) { + offset = cdata->indio_dev[index]->scan_bytes / sizeof(s64) - 1; + ((s64 *)sdata->buffer_data)[offset] = timestamp; + } + + iio_push_to_buffers(cdata->indio_dev[index], sdata->buffer_data); + + cdata->fifo_output[index].timestamp_p = timestamp; + + return 0; +} + +static void st_lsm6ds3h_parse_fifo_data(struct lsm6ds3h_data *cdata, + u16 read_len, int64_t time_top, u16 num_pattern) +{ + int err; + u16 fifo_offset = 0; + u8 gyro_sip, accel_sip; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + u8 ext0_sip; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + while (fifo_offset < read_len) { + gyro_sip = cdata->fifo_output[ST_MASK_ID_GYRO].sip; + accel_sip = cdata->fifo_output[ST_MASK_ID_ACCEL].sip; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + do { + if (gyro_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_GYRO) + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_GYRO].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp += cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp -= cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + cdata->samples_to_discard[ST_MASK_ID_GYRO] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_GYRO] > 0) + cdata->samples_to_discard[ST_MASK_ID_GYRO]--; + else { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].num_samples >= cdata->fifo_output[ST_MASK_ID_GYRO].decimator) { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_GYRO)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_GYRO] == 0) { + err = st_lsm6ds3h_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_GYRO].initialized = true; + + memcpy(cdata->gyro_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_GYRO]--; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].initialized) { + err = st_lsm6ds3h_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + cdata->gyro_last_push, + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE; + gyro_sip--; + } + + if (accel_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_ACCEL) + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp += cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp -= cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + cdata->samples_to_discard[ST_MASK_ID_ACCEL] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_ACCEL] > 0) + cdata->samples_to_discard[ST_MASK_ID_ACCEL]--; + else { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples >= cdata->fifo_output[ST_MASK_ID_ACCEL].decimator) { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] == 0) { + err = st_lsm6ds3h_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].initialized = true; + + memcpy(cdata->accel_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_ACCEL]--; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].initialized) { + err = st_lsm6ds3h_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + cdata->accel_last_push, + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE; + accel_sip--; + } + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if (ext0_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_EXT0) + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_EXT0].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp += cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp -= cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + cdata->samples_to_discard[ST_MASK_ID_EXT0] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_EXT0] > 0) + cdata->samples_to_discard[ST_MASK_ID_EXT0]--; + else { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].num_samples >= cdata->fifo_output[ST_MASK_ID_EXT0].decimator) { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_EXT0)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_EXT0] == 0) { + err = st_lsm6ds3h_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_EXT0].initialized = true; + + memcpy(cdata->ext0_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_EXT0]--; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].initialized) { + err = st_lsm6ds3h_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + cdata->ext0_last_push, + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE; + ext0_sip--; + } + + } while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0)); +#else /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } while ((accel_sip > 0) || (gyro_sip > 0)); +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } +} + +int st_lsm6ds3h_read_fifo(struct lsm6ds3h_data *cdata, bool async) +{ + int err; + u8 fifo_status[2]; +#if (CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read; +#endif /* CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO */ + u16 read_len = 0, byte_in_pattern, num_pattern; + int64_t temp_counter = 0, timestamp_diff, slower_deltatime; + + err = cdata->tf->read(cdata, ST_LSM6DS3H_FIFO_DIFF_L, + 2, fifo_status, true); + if (err < 0) + return err; + + timestamp_diff = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if (fifo_status[1] & ST_LSM6DS3H_FIFO_DATA_OVR) { + st_lsm6ds3h_set_fifo_mode(cdata, BYPASS); + st_lsm6ds3h_set_fifo_mode(cdata, CONTINUOS); + dev_err(cdata->dev, "data fifo overrun, failed to read it.\n"); + return -EINVAL; + } + + if (fifo_status[1] & ST_LSM6DS3H_FIFO_DATA_EMPTY) + return 0; + + read_len = ((fifo_status[1] & ST_LSM6DS3H_FIFO_DIFF_MASK) << 8) | fifo_status[0]; + read_len *= ST_LSM6DS3H_BYTE_FOR_CHANNEL; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip + + cdata->fifo_output[ST_MASK_ID_EXT0].sip) * + ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE; +#else /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip) * + ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + if (byte_in_pattern == 0) + return 0; + + num_pattern = read_len / byte_in_pattern; + + read_len = (read_len / byte_in_pattern) * byte_in_pattern; + if (read_len == 0) + return 0; + +#if (CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO == 0) + err = cdata->tf->read(cdata, ST_LSM6DS3H_FIFO_DATA_OUT_L, + read_len, cdata->fifo_data, true); + if (err < 0) + return err; +#else /* CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + err = cdata->tf->read(cdata, ST_LSM6DS3H_FIFO_DATA_OUT_L, + data_to_read, + &cdata->fifo_data[read_len - data_remaining], true); + if (err < 0) + return err; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO */ + + cdata->slower_id = MIN_ID(cdata->fifo_output[ST_MASK_ID_GYRO].sip, + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, + ST_MASK_ID_GYRO, ST_MASK_ID_ACCEL); +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + cdata->slower_id = MIN_ID(cdata->fifo_output[cdata->slower_id].sip, + cdata->fifo_output[ST_MASK_ID_EXT0].sip, + cdata->slower_id, ST_MASK_ID_EXT0); +#endif /* CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO */ + + temp_counter = cdata->slower_counter; + cdata->slower_counter += (read_len / byte_in_pattern) * cdata->fifo_output[cdata->slower_id].sip; + + if (async) + goto parse_fifo; + + if (temp_counter > 0) { + slower_deltatime = div64_s64(timestamp_diff - cdata->fifo_enable_timestamp, cdata->slower_counter); + + switch (cdata->slower_id) { + case ST_MASK_ID_ACCEL: + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_GYRO: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_EXT0: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = slower_deltatime; + break; + + default: + break; + } + } else { + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default; + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default; +#endif /* CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO */ + } + +parse_fifo: + st_lsm6ds3h_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern); + + return 0; +} + +int lsm6ds3h_read_output_data(struct lsm6ds3h_data *cdata, int sindex, bool push) +{ + int err; + u8 data[6]; + struct iio_dev *indio_dev = cdata->indio_dev[sindex]; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + err = cdata->tf->read(cdata, sdata->data_out_reg, + ST_LSM6DS3H_BYTE_FOR_CHANNEL * 3, data, true); + if (err < 0) + return err; + + if (push) + st_lsm6ds3h_push_data_with_timestamp(cdata, sindex, + data, cdata->timestamp); + + return 0; +} +EXPORT_SYMBOL(lsm6ds3h_read_output_data); + +static irqreturn_t st_lsm6ds3h_outdata_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static irqreturn_t st_lsm6ds3h_step_counter_trigger_handler(int irq, void *p) +{ + int err; + u8 steps_data[2]; + int64_t timestamp = 0; + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + if (!sdata->cdata->reset_steps) { + err = sdata->cdata->tf->read(sdata->cdata, + (u8)indio_dev->channels[0].address, + ST_LSM6DS3H_BYTE_FOR_CHANNEL, + steps_data, true); + if (err < 0) + goto st_lsm6ds3h_step_counter_done; + + sdata->cdata->num_steps = (sdata->cdata->num_steps & + ST_LSM6DS3H_STEP_MASK_64BIT) + *((u16 *)steps_data); + timestamp = sdata->cdata->timestamp; + } else { + sdata->cdata->num_steps = 0; + timestamp = + iio_get_time_ns(sdata->cdata->indio_dev[ST_MASK_ID_ACCEL]); + sdata->cdata->reset_steps = false; + } + + memcpy(sdata->buffer_data, (u8 *)&sdata->cdata->num_steps, sizeof(u64)); + + if (indio_dev->scan_timestamp) + *(s64 *)((u8 *)sdata->buffer_data + + ALIGN(ST_LSM6DS3H_BYTE_FOR_CHANNEL, + sizeof(s64))) = timestamp; + + iio_push_to_buffers(indio_dev, sdata->buffer_data); + +st_lsm6ds3h_step_counter_done: + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static inline irqreturn_t st_lsm6ds3h_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int st_lsm6ds3h_trig_set_state(struct iio_trigger *trig, bool state) +{ + return 0; +} + +static int st_lsm6ds3h_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) { + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + case ST_MASK_ID_GYRO: + return -EBUSY; + + default: + break; + } + } +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + return 0; +} + +static int st_lsm6ds3h_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[sdata->sindex]) && + (indio_dev->buffer->length < 2 * ST_LSM6DS3H_MAX_FIFO_LENGHT)) + return -EINVAL; + + sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); + if (!sdata->buffer_data) + return -ENOMEM; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3h_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (sdata->sindex == ST_MASK_ID_STEP_COUNTER) + iio_trigger_poll_chained(sdata->cdata->trig[ST_MASK_ID_STEP_COUNTER]); + + return 0; +} + +static int st_lsm6ds3h_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3h_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + kfree(sdata->buffer_data); + + return err < 0 ? err : 0; +} + +static const struct iio_buffer_setup_ops st_lsm6ds3h_buffer_setup_ops = { + .preenable = &st_lsm6ds3h_buffer_preenable, + .postenable = &st_lsm6ds3h_buffer_postenable, + .postdisable = &st_lsm6ds3h_buffer_postdisable, +}; + +int st_lsm6ds3h_allocate_rings(struct lsm6ds3h_data *cdata) +{ + int err; + struct lsm6ds3h_sensor_data *sdata; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_ACCEL], + NULL, &st_lsm6ds3h_outdata_trigger_handler, + &st_lsm6ds3h_buffer_setup_ops); + if (err < 0) + return err; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_GYRO]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_GYRO], + NULL, &st_lsm6ds3h_outdata_trigger_handler, + &st_lsm6ds3h_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_accel; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION], + &st_lsm6ds3h_handler_empty, NULL, + &st_lsm6ds3h_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_gyro; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER], + NULL, + &st_lsm6ds3h_step_counter_trigger_handler, + &st_lsm6ds3h_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_sign_motion; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR], + &st_lsm6ds3h_handler_empty, NULL, + &st_lsm6ds3h_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_counter; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_TILT], + &st_lsm6ds3h_handler_empty, NULL, + &st_lsm6ds3h_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_detector; + + return 0; + +buffer_cleanup_step_detector: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]); +buffer_cleanup_step_counter: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]); +buffer_cleanup_sign_motion: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]); +buffer_cleanup_gyro: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +buffer_cleanup_accel: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + return err; +} + +void st_lsm6ds3h_deallocate_rings(struct lsm6ds3h_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +} diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_core.c b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_core.c new file mode 100644 index 000000000000..26c6eb58d43c --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_core.c @@ -0,0 +1,3178 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3h core driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "st_lsm6ds3h.h" + +#define MS_TO_NS(msec) ((msec) * 1000 * 1000) + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif + +#define MIN_BNZ(a, b) (((a) < (b)) ? ((a == 0) ? \ + (b) : (a)) : ((b == 0) ? \ + (a) : (b))) + +/* COMMON VALUES FOR ACCEL-GYRO SENSORS */ +#define ST_LSM6DS3H_WAI_ADDRESS 0x0f +#define ST_LSM6DS3H_WAI_EXP 0x69 +#define ST_LSM6DS3H_INT1_ADDR 0x0d +#define ST_LSM6DS3H_INT2_ADDR 0x0e +#define ST_LSM6DS3H_ACCEL_DRDY_IRQ_MASK 0x01 +#define ST_LSM6DS3H_GYRO_DRDY_IRQ_MASK 0x02 +#define ST_LSM6DS3H_MD1_ADDR 0x5e +#define ST_LSM6DS3H_ODR_LIST_NUM 6 +#define ST_LSM6DS3H_ODR_POWER_OFF_VAL 0x00 +#define ST_LSM6DS3H_ODR_13HZ_VAL 0x01 +#define ST_LSM6DS3H_ODR_26HZ_VAL 0x02 +#define ST_LSM6DS3H_ODR_52HZ_VAL 0x03 +#define ST_LSM6DS3H_ODR_104HZ_VAL 0x04 +#define ST_LSM6DS3H_ODR_208HZ_VAL 0x05 +#define ST_LSM6DS3H_ODR_416HZ_VAL 0x06 +#define ST_LSM6DS3H_FS_LIST_NUM 4 +#define ST_LSM6DS3H_BDU_ADDR 0x12 +#define ST_LSM6DS3H_BDU_MASK 0x40 +#define ST_LSM6DS3H_EN_BIT 0x01 +#define ST_LSM6DS3H_DIS_BIT 0x00 +#define ST_LSM6DS3H_FUNC_EN_ADDR 0x19 +#define ST_LSM6DS3H_FUNC_EN_MASK 0x04 +#define ST_LSM6DS3H_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DS3H_FUNC_CFG_ACCESS_MASK 0x01 +#define ST_LSM6DS3H_FUNC_CFG_ACCESS_MASK2 0x04 +#define ST_LSM6DS3H_FUNC_CFG_REG2_MASK 0x80 +#define ST_LSM6DS3H_FUNC_CFG_START1_ADDR 0x62 +#define ST_LSM6DS3H_FUNC_CFG_START2_ADDR 0x63 +#define ST_LSM6DS3H_SENSORHUB_ADDR 0x1a +#define ST_LSM6DS3H_SENSORHUB_MASK 0x01 +#define ST_LSM6DS3H_SENSORHUB_TRIG_MASK 0x10 +#define ST_LSM6DS3H_TRIG_INTERNAL 0x00 +#define ST_LSM6DS3H_TRIG_EXTERNAL 0x01 +#define ST_LSM6DS3H_SELFTEST_ADDR 0x14 +#define ST_LSM6DS3H_SELFTEST_ACCEL_MASK 0x03 +#define ST_LSM6DS3H_SELFTEST_GYRO_MASK 0x0c +#define ST_LSM6DS3H_SELF_TEST_DISABLED_VAL 0x00 +#define ST_LSM6DS3H_SELF_TEST_POS_SIGN_VAL 0x01 +#define ST_LSM6DS3H_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 +#define ST_LSM6DS3H_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 +#define ST_LSM6DS3H_LIR_ADDR 0x58 +#define ST_LSM6DS3H_LIR_MASK 0x01 +#define ST_LSM6DS3H_TIMER_EN_ADDR 0x58 +#define ST_LSM6DS3H_TIMER_EN_MASK 0x80 +#define ST_LSM6DS3H_PEDOMETER_EN_ADDR 0x58 +#define ST_LSM6DS3H_PEDOMETER_EN_MASK 0x40 +#define ST_LSM6DS3H_INT2_ON_INT1_ADDR 0x13 +#define ST_LSM6DS3H_INT2_ON_INT1_MASK 0x20 +#define ST_LSM6DS3H_MIN_DURATION_MS 1638 +#define ST_LSM6DS3H_ROUNDING_ADDR 0x16 +#define ST_LSM6DS3H_ROUNDING_MASK 0x04 +#define ST_LSM6DS3H_FIFO_MODE_ADDR 0x0a +#define ST_LSM6DS3H_FIFO_MODE_MASK 0x07 +#define ST_LSM6DS3H_FIFO_MODE_BYPASS 0x00 +#define ST_LSM6DS3H_FIFO_MODE_CONTINUOS 0x06 +#define ST_LSM6DS3H_FIFO_THRESHOLD_IRQ_MASK 0x08 +#define ST_LSM6DS3H_FIFO_ODR_MAX 0x40 +#define ST_LSM6DS3H_FIFO_DECIMATOR_ADDR 0x08 +#define ST_LSM6DS3H_FIFO_ACCEL_DECIMATOR_MASK 0x07 +#define ST_LSM6DS3H_FIFO_GYRO_DECIMATOR_MASK 0x38 +#define ST_LSM6DS3H_FIFO_DECIMATOR2_ADDR 0x09 +#define ST_LSM6DS3H_FIFO_THR_L_ADDR 0x06 +#define ST_LSM6DS3H_FIFO_THR_H_ADDR 0x07 +#define ST_LSM6DS3H_FIFO_THR_MASK 0x0fff +#define ST_LSM6DS3H_FIFO_THR_IRQ_MASK 0x08 +#define ST_LSM6DS3H_RESET_ADDR 0x12 +#define ST_LSM6DS3H_RESET_MASK 0x01 +#define ST_LSM6DS3H_TEST_REG_ADDR 0x00 +#define ST_LSM6DS3H_START_INJECT_XL_MASK 0x08 +#define ST_LSM6DS3H_INJECT_XL_X_ADDR 0x06 +#define ST_LSM6DS3H_NS_AT_25HZ 40000000LL +#define ST_LSM6DS3H_26HZ_NS (38461538LL) +#define ST_LSM6DS3H_SELFTEST_NA_MS "na" +#define ST_LSM6DS3H_SELFTEST_FAIL_MS "fail" +#define ST_LSM6DS3H_SELFTEST_PASS_MS "pass" + +/* CUSTOM VALUES FOR ACCEL SENSOR */ +#define ST_LSM6DS3H_ACCEL_ODR_ADDR 0x10 +#define ST_LSM6DS3H_ACCEL_ODR_MASK 0xf0 +#define ST_LSM6DS3H_ACCEL_FS_ADDR 0x10 +#define ST_LSM6DS3H_ACCEL_FS_MASK 0x0c +#define ST_LSM6DS3H_ACCEL_FS_2G_VAL 0x00 +#define ST_LSM6DS3H_ACCEL_FS_4G_VAL 0x02 +#define ST_LSM6DS3H_ACCEL_FS_8G_VAL 0x03 +#define ST_LSM6DS3H_ACCEL_FS_16G_VAL 0x01 +#define ST_LSM6DS3H_ACCEL_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_LSM6DS3H_ACCEL_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_LSM6DS3H_ACCEL_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_LSM6DS3H_ACCEL_FS_16G_GAIN IIO_G_TO_M_S_2(488000) +#define ST_LSM6DS3H_ACCEL_OUT_X_L_ADDR 0x28 +#define ST_LSM6DS3H_ACCEL_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DS3H_ACCEL_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DS3H_ACCEL_STD_52HZ 1 +#define ST_LSM6DS3H_ACCEL_STD_104HZ 2 +#define ST_LSM6DS3H_ACCEL_STD_208HZ 3 +#define ST_LSM6DS3H_SELFTEST_ACCEL_ADDR 0x10 +#define ST_LSM6DS3H_SELFTEST_ACCEL_REG_VALUE 0x40 +#define ST_LSM6DS3H_SELFTEST_ACCEL_MIN 1492 +#define ST_LSM6DS3H_SELFTEST_ACCEL_MAX 27868 + +/* CUSTOM VALUES FOR GYRO SENSOR */ +#define ST_LSM6DS3H_GYRO_ODR_ADDR 0x11 +#define ST_LSM6DS3H_GYRO_ODR_MASK 0xf0 +#define ST_LSM6DS3H_GYRO_FS_ADDR 0x11 +#define ST_LSM6DS3H_GYRO_FS_MASK 0x0c +#define ST_LSM6DS3H_GYRO_FS_250_VAL 0x00 +#define ST_LSM6DS3H_GYRO_FS_500_VAL 0x01 +#define ST_LSM6DS3H_GYRO_FS_1000_VAL 0x02 +#define ST_LSM6DS3H_GYRO_FS_2000_VAL 0x03 +#define ST_LSM6DS3H_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_LSM6DS3H_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_LSM6DS3H_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_LSM6DS3H_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_LSM6DS3H_GYRO_OUT_X_L_ADDR 0x22 +#define ST_LSM6DS3H_GYRO_OUT_Y_L_ADDR 0x24 +#define ST_LSM6DS3H_GYRO_OUT_Z_L_ADDR 0x26 +#define ST_LSM6DS3H_GYRO_STD_13HZ 2 +#define ST_LSM6DS3H_GYRO_STD_52HZ 3 +#define ST_LSM6DS3H_GYRO_STD_104HZ 5 +#define ST_LSM6DS3H_GYRO_STD_208HZ 8 +#define ST_LSM6DS3H_SELFTEST_GYRO_ADDR 0x11 +#define ST_LSM6DS3H_SELFTEST_GYRO_REG_VALUE 0x4c +#define ST_LSM6DS3H_SELFTEST_GYRO_MIN 2142 +#define ST_LSM6DS3H_SELFTEST_GYRO_MAX 10000 + +/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */ +#define ST_LSM6DS3H_SIGN_MOTION_EN_ADDR 0x19 +#define ST_LSM6DS3H_SIGN_MOTION_EN_MASK 0x01 +#define ST_LSM6DS3H_SIGN_MOTION_DRDY_IRQ_MASK 0x40 + +/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ +#define ST_LSM6DS3H_STEP_DETECTOR_DRDY_IRQ_MASK 0x80 + +/* CUSTOM VALUES FOR STEP COUNTER SENSOR */ +#define ST_LSM6DS3H_STEP_COUNTER_DRDY_IRQ_MASK 0x80 +#define ST_LSM6DS3H_STEP_COUNTER_OUT_L_ADDR 0x4b +#define ST_LSM6DS3H_STEP_COUNTER_RES_ADDR 0x19 +#define ST_LSM6DS3H_STEP_COUNTER_RES_MASK 0x06 +#define ST_LSM6DS3H_STEP_COUNTER_RES_ALL_EN 0x03 +#define ST_LSM6DS3H_STEP_COUNTER_RES_FUNC_EN 0x02 +#define ST_LSM6DS3H_STEP_COUNTER_DURATION_ADDR 0x15 +#define ST_LSM6DS3H_STEP_COUNTER_THS_ADDR 0x0f +#define ST_LSM6DS3H_STEP_COUNTER_THS_2G_VALUE (0x00 | 0x10) +#define ST_LSM6DS3H_STEP_COUNTER_THS_4G_VALUE (0x80 | 0x08) + +/* CUSTOM VALUES FOR TILT SENSOR */ +#define ST_LSM6DS3H_TILT_EN_ADDR 0x58 +#define ST_LSM6DS3H_TILT_EN_MASK 0x20 +#define ST_LSM6DS3H_TILT_DRDY_IRQ_MASK 0x02 + +#define ST_LSM6DS3H_ACCEL_SUFFIX_NAME "accel" +#define ST_LSM6DS3H_GYRO_SUFFIX_NAME "gyro" +#define ST_LSM6DS3H_STEP_COUNTER_SUFFIX_NAME "step_c" +#define ST_LSM6DS3H_STEP_DETECTOR_SUFFIX_NAME "step_d" +#define ST_LSM6DS3H_SIGN_MOTION_SUFFIX_NAME "sign_motion" +#define ST_LSM6DS3H_TILT_SUFFIX_NAME "tilt" + +#define ST_LSM6DS3H_DEV_ATTR_SAMP_FREQ() \ + IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \ + st_lsm6ds3h_sysfs_get_sampling_frequency, \ + st_lsm6ds3h_sysfs_set_sampling_frequency) + +#define ST_LSM6DS3H_DEV_ATTR_SAMP_FREQ_AVAIL() \ + IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \ + st_lsm6ds3h_sysfs_sampling_frequency_avail) + +#define ST_LSM6DS3H_DEV_ATTR_SCALE_AVAIL(name) \ + IIO_DEVICE_ATTR(name, S_IRUGO, \ + st_lsm6ds3h_sysfs_scale_avail, NULL , 0); + +static struct st_lsm6ds3h_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_lsm6ds3h_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DS3H_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_LSM6DS3H_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DS3H_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DS3H_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DS3H_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DS3H_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +struct st_lsm6ds3h_odr_reg { + unsigned int hz; + u8 value; +}; + +static struct st_lsm6ds3h_odr_table { + u8 addr[2]; + u8 mask[2]; + struct st_lsm6ds3h_odr_reg odr_avl[ST_LSM6DS3H_ODR_LIST_NUM]; +} st_lsm6ds3h_odr_table = { + .addr[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_ODR_ADDR, + .mask[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_ODR_MASK, + .addr[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_ODR_ADDR, + .mask[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_ODR_MASK, + .odr_avl[0] = { .hz = 13, .value = ST_LSM6DS3H_ODR_13HZ_VAL }, + .odr_avl[1] = { .hz = 26, .value = ST_LSM6DS3H_ODR_26HZ_VAL }, + .odr_avl[2] = { .hz = 52, .value = ST_LSM6DS3H_ODR_52HZ_VAL }, + .odr_avl[3] = { .hz = 104, .value = ST_LSM6DS3H_ODR_104HZ_VAL }, + .odr_avl[4] = { .hz = 208, .value = ST_LSM6DS3H_ODR_208HZ_VAL }, + .odr_avl[5] = { .hz = 416, .value = ST_LSM6DS3H_ODR_416HZ_VAL }, +}; + +struct st_lsm6ds3h_fs_reg { + unsigned int gain; + u8 value; +}; + +static struct st_lsm6ds3h_fs_table { + u8 addr; + u8 mask; + struct st_lsm6ds3h_fs_reg fs_avl[ST_LSM6DS3H_FS_LIST_NUM]; +} st_lsm6ds3h_fs_table[ST_INDIO_DEV_NUM] = { + [ST_MASK_ID_ACCEL] = { + .addr = ST_LSM6DS3H_ACCEL_FS_ADDR, + .mask = ST_LSM6DS3H_ACCEL_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DS3H_ACCEL_FS_2G_GAIN, + .value = ST_LSM6DS3H_ACCEL_FS_2G_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DS3H_ACCEL_FS_4G_GAIN, + .value = ST_LSM6DS3H_ACCEL_FS_4G_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DS3H_ACCEL_FS_8G_GAIN, + .value = ST_LSM6DS3H_ACCEL_FS_8G_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DS3H_ACCEL_FS_16G_GAIN, + .value = ST_LSM6DS3H_ACCEL_FS_16G_VAL }, + }, + [ST_MASK_ID_GYRO] = { + .addr = ST_LSM6DS3H_GYRO_FS_ADDR, + .mask = ST_LSM6DS3H_GYRO_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DS3H_GYRO_FS_250_GAIN, + .value = ST_LSM6DS3H_GYRO_FS_250_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DS3H_GYRO_FS_500_GAIN, + .value = ST_LSM6DS3H_GYRO_FS_500_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DS3H_GYRO_FS_1000_GAIN, + .value = ST_LSM6DS3H_GYRO_FS_1000_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DS3H_GYRO_FS_2000_GAIN, + .value = ST_LSM6DS3H_GYRO_FS_2000_VAL }, + } +}; + +static const struct iio_event_spec singol_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, +}; + +const struct iio_event_spec lsm6ds3h_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lsm6ds3h_accel_ch[] = { + ST_LSM6DS3H_LSM_CHANNELS(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3H_ACCEL_OUT_X_L_ADDR, 's'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3H_ACCEL_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3H_ACCEL_OUT_Z_L_ADDR, 's'), + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6ds3h_gyro_ch[] = { + ST_LSM6DS3H_LSM_CHANNELS(IIO_ANGL_VEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3H_GYRO_OUT_X_L_ADDR, 's'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_ANGL_VEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3H_GYRO_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_ANGL_VEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3H_GYRO_OUT_Z_L_ADDR, 's'), + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_ANGL_VEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6ds3h_sign_motion_ch[] = { + { + .type = IIO_SIGN_MOTION, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3h_step_c_ch[] = { + { + .type = IIO_STEP_COUNTER, + .modified = 0, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .address = ST_LSM6DS3H_STEP_COUNTER_OUT_L_ADDR, + .scan_type = { + .sign = 'u', + .realbits = 64, + .storagebits = 64, + .endianness = IIO_LE, + }, + }, + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_STEP_COUNTER), + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3h_step_d_ch[] = { + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_STEP_DETECTOR), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + +static const struct iio_chan_spec st_lsm6ds3h_tilt_ch[] = { + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_TILT), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + + +int st_lsm6ds3h_write_data_with_mask(struct lsm6ds3h_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = cdata->tf->read(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} +EXPORT_SYMBOL(st_lsm6ds3h_write_data_with_mask); + +static inline int st_lsm6ds3h_enable_embedded_page_regs(struct lsm6ds3h_data *cdata, bool enable) +{ + u8 value = 0x00; + + if (enable) + value = ST_LSM6DS3H_FUNC_CFG_REG2_MASK; + + return cdata->tf->write(cdata, ST_LSM6DS3H_FUNC_CFG_ACCESS_ADDR, 1, &value, false); +} + +int st_lsm6ds3h_write_embedded_registers(struct lsm6ds3h_data *cdata, + u8 reg_addr, u8 *data, int len) +{ + int err = 0, err2, count = 0; + + mutex_lock(&cdata->bank_registers_lock); + + if (cdata->enable_digfunc_mask) { + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_FUNC_EN_ADDR, + ST_LSM6DS3H_FUNC_EN_MASK, + ST_LSM6DS3H_DIS_BIT, false); + if (err < 0) { + mutex_unlock(&cdata->bank_registers_lock); + return err; + } + } + + udelay(100); + + err = st_lsm6ds3h_enable_embedded_page_regs(cdata, true); + if (err < 0) + goto restore_digfunc; + + udelay(100); + + err = cdata->tf->write(cdata, reg_addr, len, data, false); + if (err < 0) + goto restore_bank_regs; + + err = st_lsm6ds3h_enable_embedded_page_regs(cdata, false); + if (err < 0) + goto restore_bank_regs; + + udelay(100); + + if (cdata->enable_digfunc_mask) { + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_FUNC_EN_ADDR, + ST_LSM6DS3H_FUNC_EN_MASK, + ST_LSM6DS3H_EN_BIT, false); + if (err < 0) + goto restore_digfunc; + } + + mutex_unlock(&cdata->bank_registers_lock); + + return 0; + +restore_bank_regs: + do { + msleep(200); + err2 = st_lsm6ds3h_enable_embedded_page_regs(cdata, false); + } while ((err2 < 0) && (count++ < 10)); + + if (count >= 10) + pr_err("not able to close embedded page registers. It make driver unstable!\n"); + +restore_digfunc: + if (!cdata->enable_digfunc_mask) { + err2 = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_FUNC_EN_ADDR, + ST_LSM6DS3H_FUNC_EN_MASK, + ST_LSM6DS3H_EN_BIT, false); + } + + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int lsm6ds3h_set_watermark(struct lsm6ds3h_data *cdata) +{ + int err; + u8 reg_value = 0; + u16 fifo_watermark; + unsigned int fifo_len, sip = 0, min_pattern = UINT_MAX; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_ACCEL] / + cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + } + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_GYRO].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_GYRO] / + cdata->fifo_output[ST_MASK_ID_GYRO].sip); + } + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_EXT0].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_EXT0] / + cdata->fifo_output[ST_MASK_ID_EXT0].sip); + } +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + if (sip == 0) + return 0; + + if (min_pattern == 0) + min_pattern = 1; + + min_pattern = MIN(min_pattern, ((unsigned int)ST_LSM6DS3H_MAX_FIFO_THRESHOLD / sip)); + + fifo_len = min_pattern * sip * ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE; + fifo_watermark = (fifo_len / 2); + + if (fifo_watermark < (ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE / 2)) + fifo_watermark = ST_LSM6DS3H_FIFO_ELEMENT_LEN_BYTE / 2; + + if (fifo_watermark != cdata->fifo_watermark) { + err = cdata->tf->read(cdata, ST_LSM6DS3H_FIFO_THR_H_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + fifo_watermark = (fifo_watermark & ST_LSM6DS3H_FIFO_THR_MASK) | + ((reg_value & ~ST_LSM6DS3H_FIFO_THR_MASK) << 8); + + err = cdata->tf->write(cdata, ST_LSM6DS3H_FIFO_THR_L_ADDR, 2, + (u8 *)&fifo_watermark, true); + if (err < 0) + return err; + + cdata->fifo_watermark = fifo_watermark; + } + + return 0; +} + +int st_lsm6ds3h_set_fifo_mode(struct lsm6ds3h_data *cdata, enum fifo_mode fm) +{ + int err; + u8 reg_value; + + switch (fm) { + case BYPASS: + reg_value = ST_LSM6DS3H_FIFO_MODE_BYPASS; + break; + case CONTINUOS: + reg_value = ST_LSM6DS3H_FIFO_MODE_CONTINUOS | ST_LSM6DS3H_FIFO_ODR_MAX; + break; + default: + return -EINVAL; + } + + err = cdata->tf->write(cdata, ST_LSM6DS3H_FIFO_MODE_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (fm != BYPASS) { + cdata->slower_counter = 0; + cdata->fifo_enable_timestamp = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = 0; + } + + cdata->fifo_status = fm; + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3h_set_fifo_mode); + +static int lsm6ds3h_write_decimators(struct lsm6ds3h_data *cdata, + u8 decimators[3]) +{ + int i; + u8 value[3], decimators_reg[2]; + + for (i = 0; i < 3; i++) { + switch (decimators[i]) { + case 0: + case 1: + case 2: + case 3: + case 4: + value[i] = decimators[i]; + break; + case 8: + value[i] = 0x05; + break; + case 16: + value[i] = 0x06; + break; + case 32: + value[i] = 0x07; + break; + default: + return -EINVAL; + } + } + + decimators_reg[0] = value[0] | (value[1] << 3); + decimators_reg[1] = value[2]; + + return cdata->tf->write(cdata, ST_LSM6DS3H_FIFO_DECIMATOR_ADDR, + ARRAY_SIZE(decimators_reg), decimators_reg, true); +} + +static bool lsm6ds3h_calculate_fifo_decimators(struct lsm6ds3h_data *cdata, + u8 decimators[3], u8 samples_in_pattern[3], + unsigned int new_v_odr[ST_INDIO_DEV_NUM + 1], + unsigned int new_hw_odr[ST_INDIO_DEV_NUM + 1], + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1], + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1]) +{ + unsigned int trigger_odr; + u8 min_decimator, max_decimator = 0; + u8 accel_decimator = 0, gyro_decimator = 0, ext_decimator = 0; + + trigger_odr = new_hw_odr[ST_MASK_ID_ACCEL]; + if (trigger_odr < new_hw_odr[ST_MASK_ID_GYRO]) + trigger_odr = new_hw_odr[ST_MASK_ID_GYRO]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_ACCEL)) && + (new_v_odr[ST_MASK_ID_ACCEL] != 0) && cdata->accel_on) + accel_decimator = trigger_odr / new_v_odr[ST_MASK_ID_ACCEL]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_GYRO)) && + (new_v_odr[ST_MASK_ID_GYRO] != 0) && + (new_hw_odr[ST_MASK_ID_GYRO] > 0)) + gyro_decimator = trigger_odr / new_v_odr[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_EXT0)) && + (new_v_odr[ST_MASK_ID_EXT0] != 0) && cdata->magn_on) + ext_decimator = trigger_odr / new_v_odr[ST_MASK_ID_EXT0]; + + new_fifo_decimator[ST_MASK_ID_EXT0] = 1; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + new_fifo_decimator[ST_MASK_ID_ACCEL] = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = 1; + + if ((accel_decimator != 0) || (gyro_decimator != 0) || (ext_decimator != 0)) { + min_decimator = MIN_BNZ(MIN_BNZ(accel_decimator, gyro_decimator), ext_decimator); + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + if (min_decimator != 1) { + if ((accel_decimator / min_decimator) == 1) { + accel_decimator = 1; + new_fifo_decimator[ST_MASK_ID_ACCEL] = min_decimator; + } else if ((gyro_decimator / min_decimator) == 1) { + gyro_decimator = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = min_decimator; + } else if ((ext_decimator / min_decimator) == 1) { + ext_decimator = 1; + new_fifo_decimator[ST_MASK_ID_EXT0] = min_decimator; + } + min_decimator = 1; + } + if ((accel_decimator > 4) && (accel_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 3; + accel_decimator = 4; + } else if ((accel_decimator > 8) && (accel_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 7; + accel_decimator = 8; + } + if ((gyro_decimator > 4) && (gyro_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 3; + gyro_decimator = 4; + } else if ((gyro_decimator > 8) && (gyro_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 7; + gyro_decimator = 8; + } +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if ((ext_decimator > 4) && (ext_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 3; + ext_decimator = 4; + } else if ((ext_decimator > 8) && (ext_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 7; + ext_decimator = 8; + } +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + } + + decimators[0] = accel_decimator; + if (accel_decimator > 0) { + new_deltatime[ST_MASK_ID_ACCEL] = accel_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[0] = max_decimator / accel_decimator; + } else + samples_in_pattern[0] = 0; + + decimators[1] = gyro_decimator; + if (gyro_decimator > 0) { + new_deltatime[ST_MASK_ID_GYRO] = gyro_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[1] = max_decimator / gyro_decimator; + } else + samples_in_pattern[1] = 0; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + decimators[2] = ext_decimator; + if (ext_decimator > 0) { + new_deltatime[ST_MASK_ID_EXT0] = ext_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[2] = max_decimator / ext_decimator; + } else + samples_in_pattern[2] = 0; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (ext_decimator == cdata->hwfifo_decimator[ST_MASK_ID_EXT0]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#else /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + return false; + } + + return true; +} + +int st_lsm6ds3h_set_drdy_irq(struct lsm6ds3h_sensor_data *sdata, bool state) +{ + int err; + u16 *irq_mask = NULL; + u8 reg_addr, mask = 0, value; + u16 tmp_irq_enable_fifo_mask, tmp_irq_enable_accel_ext_mask; + + if (state) + value = ST_LSM6DS3H_EN_BIT; + else + value = ST_LSM6DS3H_DIS_BIT; + + tmp_irq_enable_fifo_mask = + sdata->cdata->irq_enable_fifo_mask & ~sdata->sindex; + tmp_irq_enable_accel_ext_mask = + sdata->cdata->irq_enable_accel_ext_mask & ~sdata->sindex; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = ST_LSM6DS3H_INT1_ADDR; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_ACCEL]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DS3H_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_LSM6DS3H_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; + case ST_MASK_ID_GYRO: + reg_addr = ST_LSM6DS3H_INT1_ADDR; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_GYRO]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DS3H_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else + mask = ST_LSM6DS3H_GYRO_DRDY_IRQ_MASK; + + break; + case ST_MASK_ID_SIGN_MOTION: + reg_addr = ST_LSM6DS3H_INT1_ADDR; + mask = ST_LSM6DS3H_SIGN_MOTION_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_STEP_COUNTER: + reg_addr = ST_LSM6DS3H_INT2_ADDR; + mask = ST_LSM6DS3H_STEP_COUNTER_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_STEP_DETECTOR: + reg_addr = ST_LSM6DS3H_INT1_ADDR; + mask = ST_LSM6DS3H_STEP_DETECTOR_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_TILT: + reg_addr = ST_LSM6DS3H_MD1_ADDR; + mask = ST_LSM6DS3H_TILT_DRDY_IRQ_MASK; + break; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + reg_addr = ST_LSM6DS3H_INT1_ADDR; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DS3H_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_LSM6DS3H_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + default: + return -EINVAL; + } + + if (mask > 0) { + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); + if (err < 0) + return err; + } + + if (irq_mask != NULL) { + if (state) + *irq_mask |= BIT(sdata->sindex); + else + *irq_mask &= ~BIT(sdata->sindex); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3h_set_drdy_irq); + +static int st_lsm6ds3h_set_odr(struct lsm6ds3h_sensor_data *sdata, + unsigned int odr, bool force) +{ + u8 reg_value; + int err, i = 0, n; + int64_t temp_last_timestamp[3] = { 0 }; + bool scan_odr = true, fifo_conf_changed; + unsigned int temp_v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int temp_hw_odr[ST_INDIO_DEV_NUM + 1]; + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1] = { 0 }; + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1] = { 0 }; + u8 fifo_decimator[3] = { 0 }, samples_in_pattern[3] = { 0 }; + u8 temp_num_samples[3] = { 0 }, temp_old_decimator[3] = { 1 }; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + + if (scan_odr) { + for (i = 0; i < ST_LSM6DS3H_ODR_LIST_NUM; i++) { + if (st_lsm6ds3h_odr_table.odr_avl[i].hz == odr) + break; + } + if (i == ST_LSM6DS3H_ODR_LIST_NUM) + return -EINVAL; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = st_lsm6ds3h_odr_table.odr_avl[i].hz; + return 0; + } + } + + if (sdata->cdata->hw_odr[sdata->sindex] == st_lsm6ds3h_odr_table.odr_avl[i].hz) + reg_value = 0xff; + else + reg_value = st_lsm6ds3h_odr_table.odr_avl[i].value; + } else + reg_value = ST_LSM6DS3H_ODR_POWER_OFF_VAL; + + if (sdata->cdata->sensors_use_fifo > 0) { + /* someone is using fifo */ + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + temp_v_odr[ST_MASK_ID_GYRO] = sdata->cdata->v_odr[ST_MASK_ID_GYRO]; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (force) + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->accel_odr_dependency[0]; + + temp_hw_odr[ST_MASK_ID_ACCEL] = odr; + temp_hw_odr[ST_MASK_ID_GYRO] = sdata->cdata->hw_odr[ST_MASK_ID_GYRO]; + } else { + if (!force) + temp_v_odr[ST_MASK_ID_GYRO] = odr; + + temp_hw_odr[ST_MASK_ID_GYRO] = odr; + temp_hw_odr[ST_MASK_ID_ACCEL] = sdata->cdata->hw_odr[ST_MASK_ID_ACCEL]; + } +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + temp_v_odr[ST_MASK_ID_EXT0] = sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + fifo_conf_changed = lsm6ds3h_calculate_fifo_decimators(sdata->cdata, + fifo_decimator, samples_in_pattern, temp_v_odr, + temp_hw_odr, new_deltatime, new_fifo_decimator); + if (fifo_conf_changed) { + /* FIFO configuration changed, needs to write new decimators */ + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_lsm6ds3h_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + err = st_lsm6ds3h_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } + + err = lsm6ds3h_write_decimators(sdata->cdata, fifo_decimator); + if (err < 0) + goto reenable_fifo_irq; + + if (reg_value != 0xff) { + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_odr_table.addr[sdata->sindex], + st_lsm6ds3h_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_208HZ; + break; + } + } + + sdata->cdata->hwfifo_decimator[ST_MASK_ID_ACCEL] = fifo_decimator[0]; + sdata->cdata->hwfifo_decimator[ST_MASK_ID_GYRO] = fifo_decimator[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator = new_fifo_decimator[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator = new_fifo_decimator[ST_MASK_ID_GYRO]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = new_fifo_decimator[ST_MASK_ID_ACCEL] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = new_fifo_decimator[ST_MASK_ID_GYRO] - 1; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip = samples_in_pattern[0]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip = samples_in_pattern[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default = new_deltatime[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + sdata->cdata->hwfifo_decimator[ST_MASK_ID_EXT0] = fifo_decimator[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator = new_fifo_decimator[ST_MASK_ID_EXT0]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = new_fifo_decimator[ST_MASK_ID_EXT0] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip = samples_in_pattern[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + err = lsm6ds3h_set_watermark(sdata->cdata); + if (err < 0) + goto reenable_fifo_irq; + + if ((samples_in_pattern[0] > 0) || (samples_in_pattern[1] > 0) || (samples_in_pattern[2] > 0)) { + err = st_lsm6ds3h_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_lsm6ds3h_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_lsm6ds3h_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_lsm6ds3h_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } else { + /* FIFO configuration not changed */ + + if (reg_value == 0xff) { + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + return 0; + } + + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_lsm6ds3h_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + err = st_lsm6ds3h_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } + + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_odr_table.addr[sdata->sindex], + st_lsm6ds3h_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_208HZ; + break; + } + + if ((sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0)) { + err = st_lsm6ds3h_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_lsm6ds3h_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_lsm6ds3h_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_lsm6ds3h_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + } else { + /* no one is using FIFO */ + + disable_irq(sdata->cdata->irq); + + if ((odr != 0) && (sdata->cdata->hw_odr[sdata->sindex] == st_lsm6ds3h_odr_table.odr_avl[i].hz)) { + if (sdata->sindex == ST_MASK_ID_ACCEL) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + + return 0; + } + + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_odr_table.addr[sdata->sindex], + st_lsm6ds3h_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) { + enable_irq(sdata->cdata->irq); + return err; + } + + if (!force) + sdata->cdata->v_odr[sdata->sindex] = st_lsm6ds3h_odr_table.odr_avl[i].hz; + + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = st_lsm6ds3h_odr_table.odr_avl[i].hz; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (sdata->cdata->hw_odr[sdata->sindex]) { + case 13: + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DS3H_ACCEL_STD_208HZ; + break; + } + } + + switch (sdata->cdata->hw_odr[ST_MASK_ID_GYRO]) { + case 13: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_13HZ; + break; + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DS3H_GYRO_STD_208HZ; + break; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (sdata->cdata->hw_odr[sdata->sindex] > 0) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } else { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = 1; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = 1; + } + + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + sdata->cdata->trigger_odr = sdata->cdata->hw_odr[0] > sdata->cdata->hw_odr[1] ? sdata->cdata->hw_odr[0] : sdata->cdata->hw_odr[1]; + + return 0; + +reenable_fifo_irq: + enable_irq(sdata->cdata->irq); + return err; +} + +/* + * Enable / disable accelerometer + */ +static int lsm6ds3h_enable_accel(struct lsm6ds3h_data *cdata, enum st_mask_id id, int min_odr) +{ + int odr; + struct lsm6ds3h_sensor_data *sdata_accel = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + switch (id) { + case ST_MASK_ID_ACCEL: + cdata->accel_odr_dependency[0] = min_odr; + if (min_odr > 0) + cdata->accel_on = true; + else + cdata->accel_on = false; + + break; + case ST_MASK_ID_SENSOR_HUB: + cdata->accel_odr_dependency[1] = min_odr; + if (min_odr > 0) + cdata->magn_on = true; + else + cdata->magn_on = false; + + break; + case ST_MASK_ID_DIGITAL_FUNC: + cdata->accel_odr_dependency[2] = min_odr; + break; + default: + return -EINVAL; + } + + if (cdata->accel_odr_dependency[0] > cdata->accel_odr_dependency[1]) + odr = cdata->accel_odr_dependency[0]; + else + odr = cdata->accel_odr_dependency[1]; + + if (cdata->accel_odr_dependency[2] > odr) + odr = cdata->accel_odr_dependency[2]; + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + if (cdata->injection_mode) + return 0; +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + return st_lsm6ds3h_set_odr(sdata_accel, odr, true); +} + +/* + * Enable / disable digital func + */ +static int lsm6ds3h_enable_digital_func(struct lsm6ds3h_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_digfunc_mask == 0) { + err = lsm6ds3h_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 26); + if (err < 0) + return err; + + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_FUNC_EN_ADDR, + ST_LSM6DS3H_FUNC_EN_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask |= BIT(id); + } else { + if ((cdata->enable_digfunc_mask & ~BIT(id)) == 0) { + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_FUNC_EN_ADDR, + ST_LSM6DS3H_FUNC_EN_MASK, + ST_LSM6DS3H_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 0); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask &= ~BIT(id); + + } + + return 0; +} + +/* + * Enable / disable HW pedometer + */ +static int lsm6ds3h_enable_pedometer(struct lsm6ds3h_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_pedometer_mask == 0) { + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_PEDOMETER_EN_ADDR, + ST_LSM6DS3H_PEDOMETER_EN_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_digital_func(cdata, + true, ST_MASK_ID_HW_PEDOMETER); + if (err < 0) + return err; + } + cdata->enable_pedometer_mask |= BIT(id); + } else { + if ((cdata->enable_pedometer_mask & ~BIT(id)) == 0) { + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_PEDOMETER_EN_ADDR, + ST_LSM6DS3H_PEDOMETER_EN_MASK, + ST_LSM6DS3H_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_digital_func(cdata, + false, ST_MASK_ID_HW_PEDOMETER); + if (err < 0) + return err; + } + cdata->enable_pedometer_mask &= ~BIT(id); + } + + return 0; +} + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT +int st_lsm6ds3h_enable_sensor_hub(struct lsm6ds3h_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_sensorhub_mask == 0) { + err = lsm6ds3h_enable_digital_func(cdata, + true, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + + err = lsm6ds3h_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + if (err < 0) + return err; + + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_SENSORHUB_ADDR, + ST_LSM6DS3H_SENSORHUB_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + } else + err = lsm6ds3h_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask |= BIT(id); + } else { + if ((cdata->enable_sensorhub_mask & ~BIT(id)) == 0) { + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_SENSORHUB_ADDR, + ST_LSM6DS3H_SENSORHUB_MASK, + ST_LSM6DS3H_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_accel(cdata, + ST_MASK_ID_SENSOR_HUB, 0); + if (err < 0) + return err; + + err = lsm6ds3h_enable_digital_func(cdata, + false, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + } else + err = lsm6ds3h_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask &= ~BIT(id); + } + + return err < 0 ? err : 0; +} +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + +int st_lsm6ds3h_set_enable(struct lsm6ds3h_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + err = lsm6ds3h_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, + enable ? sdata->cdata->v_odr[ST_MASK_ID_ACCEL] : 0); + if (err < 0) + return 0; + + break; + case ST_MASK_ID_GYRO: + err = st_lsm6ds3h_set_odr(sdata, enable ? + sdata->cdata->v_odr[ST_MASK_ID_GYRO] : 0, true); + if (err < 0) + return err; + + break; + case ST_MASK_ID_SIGN_MOTION: + if (enable) + reg_value = ST_LSM6DS3H_EN_BIT; + else + reg_value = ST_LSM6DS3H_DIS_BIT; + + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_SIGN_MOTION_EN_ADDR, + ST_LSM6DS3H_SIGN_MOTION_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_SIGN_MOTION); + if (err < 0) + return err; + + break; + case ST_MASK_ID_STEP_COUNTER: + if (enable) + reg_value = ST_LSM6DS3H_EN_BIT; + else + reg_value = ST_LSM6DS3H_DIS_BIT; + + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_TIMER_EN_ADDR, + ST_LSM6DS3H_TIMER_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_STEP_COUNTER); + if (err < 0) + return err; + + break; + case ST_MASK_ID_STEP_DETECTOR: + err = lsm6ds3h_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_STEP_DETECTOR); + if (err < 0) + return err; + + break; + case ST_MASK_ID_TILT: + if (enable) + reg_value = ST_LSM6DS3H_EN_BIT; + else + reg_value = ST_LSM6DS3H_DIS_BIT; + + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_TILT_EN_ADDR, + ST_LSM6DS3H_TILT_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6ds3h_enable_digital_func(sdata->cdata, + enable, ST_MASK_ID_TILT); + if (err < 0) + return err; + + break; + default: + return -EINVAL; + } + + if (buffer) { + err = st_lsm6ds3h_set_drdy_irq(sdata, enable); + if (err < 0) + return err; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; +} + +static int st_lsm6ds3h_set_fs(struct lsm6ds3h_sensor_data *sdata, + unsigned int gain) +{ + int err, i; + u8 pedometer_reg_value; + + for (i = 0; i < ST_LSM6DS3H_FS_LIST_NUM; i++) { + if (st_lsm6ds3h_fs_table[sdata->sindex].fs_avl[i].gain == gain) + break; + } + if (i == ST_LSM6DS3H_FS_LIST_NUM) + return -EINVAL; + + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_fs_table[sdata->sindex].addr, + st_lsm6ds3h_fs_table[sdata->sindex].mask, + st_lsm6ds3h_fs_table[sdata->sindex].fs_avl[i].value, + true); + if (err < 0) + return err; + + sdata->c_gain[0] = gain; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (i == 0) + pedometer_reg_value = ST_LSM6DS3H_STEP_COUNTER_THS_2G_VALUE; + else + pedometer_reg_value = ST_LSM6DS3H_STEP_COUNTER_THS_4G_VALUE; + + st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_STEP_COUNTER_THS_ADDR, + &pedometer_reg_value, 1); + } + + return 0; +} + +static int st_lsm6ds3h_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[ST_LSM6DS3H_BYTE_FOR_CHANNEL]; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3h_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) + msleep(40); + + if (sdata->sindex == ST_MASK_ID_GYRO) + msleep(120); + + err = sdata->cdata->tf->read(sdata->cdata, ch->address, + ST_LSM6DS3H_BYTE_FOR_CHANNEL, outdata, true); + if (err < 0) { + st_lsm6ds3h_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + st_lsm6ds3h_set_enable(sdata, false, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[0]; + return IIO_VAL_INT_PLUS_NANO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6ds3h_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long mask) +{ + int err; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = st_lsm6ds3h_set_fs(sdata, val2); + mutex_unlock(&indio_dev->mlock); + break; + default: + return -EINVAL; + } + + return err < 0 ? err : 0; +} + +static int st_lsm6ds3h_reset_steps(struct lsm6ds3h_data *cdata) +{ + int err; + u8 reg_value = 0x00; + + err = cdata->tf->read(cdata, + ST_LSM6DS3H_STEP_COUNTER_RES_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (reg_value & ST_LSM6DS3H_FUNC_EN_MASK) + reg_value = ST_LSM6DS3H_STEP_COUNTER_RES_FUNC_EN; + else + reg_value = ST_LSM6DS3H_DIS_BIT; + + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_STEP_COUNTER_RES_ADDR, + ST_LSM6DS3H_STEP_COUNTER_RES_MASK, + ST_LSM6DS3H_STEP_COUNTER_RES_ALL_EN, true); + if (err < 0) + return err; + + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_STEP_COUNTER_RES_ADDR, + ST_LSM6DS3H_STEP_COUNTER_RES_MASK, + reg_value, true); + if (err < 0) + return err; + + cdata->reset_steps = true; + + return 0; +} + +static int st_lsm6ds3h_init_sensor(struct lsm6ds3h_data *cdata) +{ + int err; + u8 default_reg_value = ST_LSM6DS3H_RESET_MASK; + + err = cdata->tf->write(cdata, ST_LSM6DS3H_RESET_ADDR, 1, + &default_reg_value, true); + if (err < 0) + return err; + + msleep(200); + + /* Latch interrupts */ + err = st_lsm6ds3h_write_data_with_mask(cdata, ST_LSM6DS3H_LIR_ADDR, + ST_LSM6DS3H_LIR_MASK, ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + /* Enable BDU for sensors data */ + err = st_lsm6ds3h_write_data_with_mask(cdata, ST_LSM6DS3H_BDU_ADDR, + ST_LSM6DS3H_BDU_MASK, ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_ROUNDING_ADDR, + ST_LSM6DS3H_ROUNDING_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + /* Redirect INT2 on INT1, all interrupt will be available on INT1 */ + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_INT2_ON_INT1_ADDR, + ST_LSM6DS3H_INT2_ON_INT1_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3h_reset_steps(cdata); + if (err < 0) + return err; + + default_reg_value = 0x00; + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_STEP_COUNTER_DURATION_ADDR, + &default_reg_value, 1); + if (err < 0) + return err; + + default_reg_value = ST_LSM6DS3H_STEP_COUNTER_THS_2G_VALUE; + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_STEP_COUNTER_THS_ADDR, + &default_reg_value, 1); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6ds3h_set_selftest(struct lsm6ds3h_sensor_data *sdata, int index) +{ + u8 mode, mask; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + mask = ST_LSM6DS3H_SELFTEST_ACCEL_MASK; + mode = st_lsm6ds3h_selftest_table[index].accel_value; + break; + case ST_MASK_ID_GYRO: + mask = ST_LSM6DS3H_SELFTEST_GYRO_MASK; + mode = st_lsm6ds3h_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_SELFTEST_ADDR, mask, mode, true); +} + +static ssize_t st_lsm6ds3h_sysfs_set_max_delivery_rate(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u8 duration; + int err; + unsigned int max_delivery_rate; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtouint(buf, 10, &max_delivery_rate); + if (err < 0) + return -EINVAL; + + if (max_delivery_rate == sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER]) + return size; + + duration = max_delivery_rate / ST_LSM6DS3H_MIN_DURATION_MS; + + err = st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_STEP_COUNTER_DURATION_ADDR, + &duration, 1); + if (err < 0) + return err; + + sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER] = max_delivery_rate; + + return size; +} + +static ssize_t st_lsm6ds3h_sysfs_get_max_delivery_rate(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3h_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", + sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER]); +} + +static ssize_t st_lsm6ds3h_sysfs_reset_counter(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + struct lsm6ds3h_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + err = st_lsm6ds3h_reset_steps(sdata->cdata); + if (err < 0) + return err; + + return size; +} + +static ssize_t st_lsm6ds3h_sysfs_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3h_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_lsm6ds3h_sysfs_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + + mutex_lock(&sdata->cdata->odr_lock); +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + if (!((sdata->sindex & ST_MASK_ID_ACCEL) && + sdata->cdata->injection_mode)) { + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_lsm6ds3h_set_odr(sdata, odr, false); + } +#else /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + if (sdata->cdata->v_odr[sdata->sindex] != odr) { + if ((sdata->sindex == ST_MASK_ID_ACCEL) && (sdata->cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL))) + err = lsm6ds3h_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, odr); + else + err = st_lsm6ds3h_set_odr(sdata, odr, false); + } +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + mutex_unlock(&sdata->cdata->odr_lock); + + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6ds3h_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < ST_LSM6DS3H_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lsm6ds3h_odr_table.odr_avl[i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6ds3h_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + struct lsm6ds3h_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + for (i = 0; i < ST_LSM6DS3H_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_lsm6ds3h_fs_table[sdata->sindex].fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6ds3h_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6ds3h_selftest_table[1].string_mode, + st_lsm6ds3h_selftest_table[2].string_mode); +} + +static ssize_t st_lsm6ds3h_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + result = sdata->cdata->accel_selftest_status; + break; + case ST_MASK_ID_GYRO: + result = sdata->cdata->gyro_selftest_status; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_LSM6DS3H_SELFTEST_NA_MS; + else if (result < 0) + message = ST_LSM6DS3H_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_LSM6DS3H_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_lsm6ds3h_sysfs_start_selftest_status(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i, n; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + u8 reg_status, reg_addr, temp_reg_status, outdata[6]; + int x = 0, y = 0, z = 0, x_selftest = 0, y_selftest = 0, z_selftest = 0; + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + sdata->cdata->accel_selftest_status = 0; + break; + case ST_MASK_ID_GYRO: + sdata->cdata->gyro_selftest_status = 0; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + for (n = 0; n < ARRAY_SIZE(st_lsm6ds3h_selftest_table); n++) { + if (strncmp(buf, st_lsm6ds3h_selftest_table[n].string_mode, + size - 2) == 0) + break; + } + if (n == ARRAY_SIZE(st_lsm6ds3h_selftest_table)) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = ST_LSM6DS3H_SELFTEST_ACCEL_ADDR; + temp_reg_status = ST_LSM6DS3H_SELFTEST_ACCEL_REG_VALUE; + break; + case ST_MASK_ID_GYRO: + reg_addr = ST_LSM6DS3H_SELFTEST_GYRO_ADDR; + temp_reg_status = ST_LSM6DS3H_SELFTEST_GYRO_REG_VALUE; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = sdata->cdata->tf->read(sdata->cdata, + reg_addr, 1, ®_status, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, &temp_reg_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 20; + y += ((s16)*(u16 *)&outdata[2]) / 20; + z += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = st_lsm6ds3h_set_selftest(sdata, n); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest enabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 20; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 20; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, ®_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = st_lsm6ds3h_set_selftest(sdata, 0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + if ((abs(x_selftest - x) < ST_LSM6DS3H_SELFTEST_ACCEL_MIN) || + (abs(x_selftest - x) > ST_LSM6DS3H_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_LSM6DS3H_SELFTEST_ACCEL_MIN) || + (abs(y_selftest - y) > ST_LSM6DS3H_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_LSM6DS3H_SELFTEST_ACCEL_MIN) || + (abs(z_selftest - z) > ST_LSM6DS3H_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->accel_selftest_status = 1; + break; + case ST_MASK_ID_GYRO: + if ((abs(x_selftest - x) < ST_LSM6DS3H_SELFTEST_GYRO_MIN) || + (abs(x_selftest - x) > ST_LSM6DS3H_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_LSM6DS3H_SELFTEST_GYRO_MIN) || + (abs(y_selftest - y) > ST_LSM6DS3H_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_LSM6DS3H_SELFTEST_GYRO_MIN) || + (abs(z_selftest - z) > ST_LSM6DS3H_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->gyro_selftest_status = 1; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + +selftest_failure: + mutex_unlock(&sdata->cdata->odr_lock); + + return size; +} + +ssize_t st_lsm6ds3h_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u64 sensor_last_timestamp, event_type = 0; + int stype = 0; + u64 timestamp_flush = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_lock(&sdata->cdata->odr_lock); + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = + sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + st_lsm6ds3h_read_fifo(sdata->cdata, true); + + if (sensor_last_timestamp == + sdata->cdata->fifo_output[sdata->sindex].timestamp_p) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + timestamp_flush = sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + enable_irq(sdata->cdata->irq); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + stype = IIO_ACCEL; + break; + + case ST_MASK_ID_GYRO: + stype = IIO_ANGL_VEL; + break; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + stype = IIO_MAGN; + break; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + } + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(stype, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + timestamp_flush); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_enabled[sdata->sindex]); +} + +ssize_t st_lsm6ds3h_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + bool enable = false; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + err = -EBUSY; + goto set_hwfifo_enabled_unlock_mutex; + } + + err = strtobool(buf, &enable); + if (err < 0) + goto set_hwfifo_enabled_unlock_mutex; + + mutex_lock(&sdata->cdata->odr_lock); + + sdata->cdata->hwfifo_enabled[sdata->sindex] = enable; + + if (enable) + sdata->cdata->sensors_use_fifo |= BIT(sdata->sindex); + else + sdata->cdata->sensors_use_fifo &= ~BIT(sdata->sindex); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; + +set_hwfifo_enabled_unlock_mutex: + mutex_unlock(&indio_dev->mlock); + return err; +} + +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_watermark[sdata->sindex]); +} + +ssize_t st_lsm6ds3h_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err = 0, watermark = 0, old_watermark; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > ST_LSM6DS3H_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) && + (sdata->cdata->sensors_use_fifo & BIT(sdata->sindex))) { + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) + st_lsm6ds3h_read_fifo(sdata->cdata, true); + + old_watermark = sdata->cdata->hwfifo_watermark[sdata->sindex]; + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + err = lsm6ds3h_set_watermark(sdata->cdata); + if (err < 0) + sdata->cdata->hwfifo_watermark[sdata->sindex] = old_watermark; + + enable_irq(sdata->cdata->irq); + } else + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : size; +} + +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", ST_LSM6DS3H_MAX_FIFO_LENGHT); +} + +ssize_t st_lsm6ds3h_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION +static ssize_t st_lsm6ds3h_sysfs_set_injection_mode(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, start; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = kstrtoint(buf, 10, &start); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + mutex_lock(&sdata->cdata->odr_lock); + + if (start == 0) { + hrtimer_cancel(&sdata->cdata->injection_timer); + + /* End injection */ + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_TEST_REG_ADDR, + ST_LSM6DS3H_START_INJECT_XL_MASK, 0, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + /* Force accel ODR to 26Hz if dependencies are enabled */ + if (sdata->cdata->sensors_enabled > 0) { + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_odr_table.addr[sdata->sindex], + st_lsm6ds3h_odr_table.mask[sdata->sindex], + st_lsm6ds3h_odr_table.odr_avl[1].value, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + } + + sdata->cdata->injection_mode = false; + } else { + sdata->cdata->last_injection_timestamp = 0; + sdata->cdata->injection_samples = 0; + + /* Force accel ODR to 26Hz */ + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_odr_table.addr[sdata->sindex], + st_lsm6ds3h_odr_table.mask[sdata->sindex], + st_lsm6ds3h_odr_table.odr_avl[1].value, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + /* Set start injection */ + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_TEST_REG_ADDR, + ST_LSM6DS3H_START_INJECT_XL_MASK, 1, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + sdata->cdata->injection_mode = true; + } + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static ssize_t st_lsm6ds3h_sysfs_get_injection_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->injection_mode); +} + +static ssize_t st_lsm6ds3h_sysfs_upload_xl_data(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int i; + u8 sample[3]; + s64 timestamp; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (!sdata->cdata->injection_mode) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + for (i = 0; i < 3; i++) + sample[i] = *(s16 *)(&buf[i * 2]) >> 8; + + timestamp = *(s64 *)(buf + ALIGN(6, sizeof(s64))); + + if (timestamp < sdata->cdata->last_injection_timestamp + + ST_LSM6DS3H_NS_AT_25HZ) { + mutex_unlock(&indio_dev->mlock); + return size; + } + + while (sdata->cdata->injection_samples >= 10) + msleep(200); + + spin_lock(&sdata->cdata->injection_spinlock); + + memcpy(&sdata->cdata->injection_data[ + sdata->cdata->injection_samples * 3], sample, 3); + sdata->cdata->injection_samples++; + + spin_unlock(&sdata->cdata->injection_spinlock); + + sdata->cdata->last_injection_timestamp = timestamp; + + if (sdata->cdata->injection_samples >= 8) + hrtimer_start(&sdata->cdata->injection_timer, + ktime_set(0, ST_LSM6DS3H_26HZ_NS), HRTIMER_MODE_REL); + + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static void st_lsm6ds3h_injection_work(struct work_struct *work) +{ + int i, err; + struct lsm6ds3h_data *cdata; + + cdata = container_of(work, struct lsm6ds3h_data, injection_work); + + if (cdata->injection_samples == 0) + return; + + err = cdata->tf->write(cdata, ST_LSM6DS3H_INJECT_XL_X_ADDR, + 3, cdata->injection_data, false); + if (err < 0) + return; + + spin_lock(&cdata->injection_spinlock); + + for (i = 0; i < cdata->injection_samples - 1; i++) + memcpy(&cdata->injection_data[i * 3], + &cdata->injection_data[(i + 1) * 3], 3); + + cdata->injection_samples--; + + spin_unlock(&cdata->injection_spinlock); +} + +static enum hrtimer_restart st_lsm6ds3h_injection_timer_func( + struct hrtimer *timer) +{ + ktime_t now; + struct lsm6ds3h_data *cdata; + + cdata = container_of(timer, struct lsm6ds3h_data, injection_timer); + + now = hrtimer_cb_get_time(timer); + hrtimer_forward(timer, now, ktime_set(0, ST_LSM6DS3H_26HZ_NS)); + + schedule_work(&cdata->injection_work); + + return HRTIMER_RESTART; +} + +static ssize_t st_lsm6ds3h_sysfs_get_injection_sensors(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", "lsm6ds3h_accel"); +} +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + +ssize_t st_lsm6ds3h_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + struct lsm6ds3h_data *cdata = sdata->cdata; + + return scnprintf(buf, PAGE_SIZE, "%u\n", cdata->module_id); +} + +static ST_LSM6DS3H_DEV_ATTR_SAMP_FREQ(); +static ST_LSM6DS3H_DEV_ATTR_SAMP_FREQ_AVAIL(); +static ST_LSM6DS3H_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); +static ST_LSM6DS3H_DEV_ATTR_SCALE_AVAIL(in_anglvel_scale_available); + +static ST_LSM6DS3H_HWFIFO_ENABLED(); +static ST_LSM6DS3H_HWFIFO_WATERMARK(); +static ST_LSM6DS3H_HWFIFO_WATERMARK_MIN(); +static ST_LSM6DS3H_HWFIFO_WATERMARK_MAX(); +static ST_LSM6DS3H_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(reset_counter, S_IWUSR, + NULL, st_lsm6ds3h_sysfs_reset_counter, 0); + +static IIO_DEVICE_ATTR(max_delivery_rate, S_IWUSR | S_IRUGO, + st_lsm6ds3h_sysfs_get_max_delivery_rate, + st_lsm6ds3h_sysfs_set_max_delivery_rate, 0); + +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_lsm6ds3h_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_lsm6ds3h_sysfs_get_selftest_status, + st_lsm6ds3h_sysfs_start_selftest_status, 0); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6ds3h_get_module_id, NULL, 0); + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION +static IIO_DEVICE_ATTR(injection_mode, S_IWUSR | S_IRUGO, + st_lsm6ds3h_sysfs_get_injection_mode, + st_lsm6ds3h_sysfs_set_injection_mode, 0); + +static IIO_DEVICE_ATTR(in_accel_injection_raw, S_IWUSR, NULL, + st_lsm6ds3h_sysfs_upload_xl_data, 0); + +static IIO_DEVICE_ATTR(injection_sensors, S_IRUGO, + st_lsm6ds3h_sysfs_get_injection_sensors, + NULL, 0); +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + +static int st_lsm6ds3h_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == IIO_CHAN_INFO_SCALE) { + if ((chan->type == IIO_ANGL_VEL) || + (chan->type == IIO_ACCEL)) + return IIO_VAL_INT_PLUS_NANO; + } + + return -EINVAL; +} + +static struct attribute *st_lsm6ds3h_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + &iio_dev_attr_injection_mode.dev_attr.attr, + &iio_dev_attr_in_accel_injection_raw.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3h_accel_attribute_group = { + .attrs = st_lsm6ds3h_accel_attributes, +}; + +static const struct iio_info st_lsm6ds3h_accel_info = { + .attrs = &st_lsm6ds3h_accel_attribute_group, + .read_raw = &st_lsm6ds3h_read_raw, + .write_raw = &st_lsm6ds3h_write_raw, + .write_raw_get_fmt = st_lsm6ds3h_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6ds3h_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.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_lsm6ds3h_gyro_attribute_group = { + .attrs = st_lsm6ds3h_gyro_attributes, +}; + +static const struct iio_info st_lsm6ds3h_gyro_info = { + .attrs = &st_lsm6ds3h_gyro_attribute_group, + .read_raw = &st_lsm6ds3h_read_raw, + .write_raw = &st_lsm6ds3h_write_raw, + .write_raw_get_fmt = st_lsm6ds3h_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6ds3h_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3h_sign_motion_attribute_group = { + .attrs = st_lsm6ds3h_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6ds3h_sign_motion_info = { + .attrs = &st_lsm6ds3h_sign_motion_attribute_group, +}; + +static struct attribute *st_lsm6ds3h_step_c_attributes[] = { + &iio_dev_attr_reset_counter.dev_attr.attr, + &iio_dev_attr_max_delivery_rate.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3h_step_c_attribute_group = { + .attrs = st_lsm6ds3h_step_c_attributes, +}; + +static const struct iio_info st_lsm6ds3h_step_c_info = { + .attrs = &st_lsm6ds3h_step_c_attribute_group, + .read_raw = &st_lsm6ds3h_read_raw, +}; + +static struct attribute *st_lsm6ds3h_step_d_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3h_step_d_attribute_group = { + .attrs = st_lsm6ds3h_step_d_attributes, +}; + +static const struct iio_info st_lsm6ds3h_step_d_info = { + .attrs = &st_lsm6ds3h_step_d_attribute_group, +}; + +static struct attribute *st_lsm6ds3h_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3h_tilt_attribute_group = { + .attrs = st_lsm6ds3h_tilt_attributes, +}; + +static const struct iio_info st_lsm6ds3h_tilt_info = { + .attrs = &st_lsm6ds3h_tilt_attribute_group, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops st_lsm6ds3h_trigger_ops = { + .set_trigger_state = ST_LSM6DS3H_TRIGGER_SET_STATE, +}; +#define ST_LSM6DS3H_TRIGGER_OPS (&st_lsm6ds3h_trigger_ops) +#else +#define ST_LSM6DS3H_TRIGGER_OPS NULL +#endif + +static void st_lsm6ds3h_get_properties(struct lsm6ds3h_data *cdata) +{ + if (device_property_read_u32(cdata->dev, "st,module_id", + &cdata->module_id)) { + cdata->module_id = 1; + } +} + +int st_lsm6ds3h_common_probe(struct lsm6ds3h_data *cdata, int irq) +{ + u8 wai = 0x00; + int i, n, err; + struct lsm6ds3h_sensor_data *sdata; + + mutex_init(&cdata->bank_registers_lock); + mutex_init(&cdata->fifo_lock); + mutex_init(&cdata->tb.buf_lock); + mutex_init(&cdata->odr_lock); + + cdata->fifo_watermark = 0; + cdata->fifo_status = BYPASS; + cdata->enable_digfunc_mask = 0; + cdata->enable_pedometer_mask = 0; +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + cdata->enable_sensorhub_mask = 0; +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + cdata->irq_enable_fifo_mask = 0; + cdata->irq_enable_accel_ext_mask = 0; + + for (i = 0; i < ST_INDIO_DEV_NUM + 1; i++) { + cdata->hw_odr[i] = 0; + cdata->v_odr[i] = 0; + cdata->hwfifo_enabled[i] = false; + cdata->hwfifo_decimator[i] = 0; + cdata->hwfifo_watermark[i] = 1; + cdata->nofifo_decimation[i].decimator = 1; + cdata->nofifo_decimation[i].num_samples = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].decimator = 1; + cdata->fifo_output[i].timestamp_p = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].initialized = false; + } + + cdata->sensors_use_fifo = 0; + cdata->sensors_enabled = 0; + + cdata->gyro_selftest_status = 0; + cdata->accel_selftest_status = 0; + + cdata->accel_on = false; + cdata->magn_on = false; + + cdata->reset_steps = false; + cdata->num_steps = 0; + + cdata->accel_odr_dependency[0] = 0; + cdata->accel_odr_dependency[1] = 0; + cdata->accel_odr_dependency[2] = 0; + + cdata->trigger_odr = 0; + + cdata->fifo_data = kmalloc(ST_LSM6DS3H_MAX_FIFO_SIZE * + sizeof(u8), GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + cdata->injection_mode = false; + cdata->last_injection_timestamp = 0; + + INIT_WORK(&cdata->injection_work, &st_lsm6ds3h_injection_work); + hrtimer_init(&cdata->injection_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + cdata->injection_timer.function = &st_lsm6ds3h_injection_timer_func; + spin_lock_init(&cdata->injection_spinlock); +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + err = cdata->tf->read(cdata, ST_LSM6DS3H_WAI_ADDRESS, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + goto free_fifo_data; + } + if (wai != ST_LSM6DS3H_WAI_EXP) { + dev_err(cdata->dev, + "Who-Am-I value not valid. Expected %x, Found %x\n", + ST_LSM6DS3H_WAI_EXP, wai); + err = -ENODEV; + goto free_fifo_data; + } + + st_lsm6ds3h_get_properties(cdata); + + if (irq > 0) { + cdata->irq = irq; + dev_info(cdata->dev, "driver use DRDY int pin 1.\n"); + } else { + err = -EINVAL; + dev_info(cdata->dev, + "DRDY not available, curernt implementation needs irq!\n"); + goto free_fifo_data; + } + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + cdata->indio_dev[i] = devm_iio_device_alloc(cdata->dev, + sizeof(struct lsm6ds3h_sensor_data)); + if (!cdata->indio_dev[i]) { + err = -ENOMEM; + goto free_fifo_data; + } + + sdata = iio_priv(cdata->indio_dev[i]); + sdata->cdata = cdata; + sdata->sindex = i; + + switch (i) { + case ST_MASK_ID_ACCEL: + sdata->data_out_reg = st_lsm6ds3h_accel_ch[0].address; + cdata->v_odr[i] = st_lsm6ds3h_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_lsm6ds3h_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_GYRO: + sdata->data_out_reg = st_lsm6ds3h_gyro_ch[0].address; + cdata->v_odr[i] = st_lsm6ds3h_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_lsm6ds3h_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_STEP_COUNTER: + sdata->data_out_reg = st_lsm6ds3h_step_c_ch[0].address; + sdata->num_data_channels = 1; + break; + + default: + sdata->num_data_channels = 0; + break; + } + + cdata->indio_dev[i]->modes = INDIO_DIRECT_MODE; + } + + cdata->indio_dev[ST_MASK_ID_ACCEL]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3H_ACCEL_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_ACCEL]->info = &st_lsm6ds3h_accel_info; + cdata->indio_dev[ST_MASK_ID_ACCEL]->channels = st_lsm6ds3h_accel_ch; + cdata->indio_dev[ST_MASK_ID_ACCEL]->num_channels = + ARRAY_SIZE(st_lsm6ds3h_accel_ch); + + cdata->indio_dev[ST_MASK_ID_GYRO]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3H_GYRO_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_GYRO]->info = &st_lsm6ds3h_gyro_info; + cdata->indio_dev[ST_MASK_ID_GYRO]->channels = st_lsm6ds3h_gyro_ch; + cdata->indio_dev[ST_MASK_ID_GYRO]->num_channels = + ARRAY_SIZE(st_lsm6ds3h_gyro_ch); + + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3H_SIGN_MOTION_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->info = + &st_lsm6ds3h_sign_motion_info; + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->channels = + st_lsm6ds3h_sign_motion_ch; + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->num_channels = + ARRAY_SIZE(st_lsm6ds3h_sign_motion_ch); + + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3H_STEP_COUNTER_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->info = + &st_lsm6ds3h_step_c_info; + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->channels = + st_lsm6ds3h_step_c_ch; + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->num_channels = + ARRAY_SIZE(st_lsm6ds3h_step_c_ch); + + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3H_STEP_DETECTOR_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->info = + &st_lsm6ds3h_step_d_info; + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->channels = + st_lsm6ds3h_step_d_ch; + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->num_channels = + ARRAY_SIZE(st_lsm6ds3h_step_d_ch); + + cdata->indio_dev[ST_MASK_ID_TILT]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3H_TILT_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_TILT]->info = &st_lsm6ds3h_tilt_info; + cdata->indio_dev[ST_MASK_ID_TILT]->channels = st_lsm6ds3h_tilt_ch; + cdata->indio_dev[ST_MASK_ID_TILT]->num_channels = + ARRAY_SIZE(st_lsm6ds3h_tilt_ch); + + err = st_lsm6ds3h_init_sensor(cdata); + if (err < 0) + goto free_fifo_data; + + err = st_lsm6ds3h_allocate_rings(cdata); + if (err < 0) + goto free_fifo_data; + + if (irq > 0) { + err = st_lsm6ds3h_allocate_triggers(cdata, + ST_LSM6DS3H_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_device_register(cdata->indio_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + st_lsm6ds3h_i2c_master_probe(cdata); + + device_init_wakeup(cdata->dev, true); + + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->indio_dev[n]); + + if (irq > 0) + st_lsm6ds3h_deallocate_triggers(cdata); +deallocate_ring: + st_lsm6ds3h_deallocate_rings(cdata); +free_fifo_data: + kfree(cdata->fifo_data); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3h_common_probe); + +void st_lsm6ds3h_common_remove(struct lsm6ds3h_data *cdata, int irq) +{ + int i; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_device_unregister(cdata->indio_dev[i]); + + if (irq > 0) + st_lsm6ds3h_deallocate_triggers(cdata); + + st_lsm6ds3h_deallocate_rings(cdata); + + kfree(cdata->fifo_data); + + st_lsm6ds3h_i2c_master_exit(cdata); +} +EXPORT_SYMBOL(st_lsm6ds3h_common_remove); + +#ifdef CONFIG_PM +int __maybe_unused st_lsm6ds3h_common_suspend(struct lsm6ds3h_data *cdata) +{ + int err, i; + u8 tmp_sensors_enabled; + struct lsm6ds3h_sensor_data *sdata; + + tmp_sensors_enabled = cdata->sensors_enabled; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_MASK_ID_SIGN_MOTION) || (i == ST_MASK_ID_TILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + +#ifdef CONFIG_ST_LSM6DS3H_STEP_COUNTER_ON_DURING_SUSPEND + if ((BIT(i) & cdata->sensors_enabled) && + (i == ST_MASK_ID_STEP_COUNTER)) { + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_INT2_ADDR, + ST_LSM6DS3H_STEP_COUNTER_DRDY_IRQ_MASK, + ST_LSM6DS3H_DIS_BIT, true); + if (err < 0) + return err; + + continue; + } +#endif /* CONFIG_ST_LSM6DS3H_STEP_COUNTER_ON_DURING_SUSPEND */ + + err = st_lsm6ds3h_set_enable(sdata, false, true); + if (err < 0) + return err; + } + cdata->sensors_enabled = tmp_sensors_enabled; + + if (cdata->sensors_enabled & ST_LSM6DS3H_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + enable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3h_common_suspend); + +int __maybe_unused st_lsm6ds3h_common_resume(struct lsm6ds3h_data *cdata) +{ + int err, i; + struct lsm6ds3h_sensor_data *sdata; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_MASK_ID_SIGN_MOTION) || (i == ST_MASK_ID_TILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + + if (BIT(sdata->sindex) & cdata->sensors_enabled) { +#ifdef CONFIG_ST_LSM6DS3H_STEP_COUNTER_ON_DURING_SUSPEND + if (i == ST_MASK_ID_STEP_COUNTER) { + err = st_lsm6ds3h_write_data_with_mask(sdata->cdata, + ST_LSM6DS3H_INT2_ADDR, + ST_LSM6DS3H_STEP_COUNTER_DRDY_IRQ_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; + + continue; + } +#endif /* CONFIG_ST_LSM6DS3H_STEP_COUNTER_ON_DURING_SUSPEND */ + + err = st_lsm6ds3h_set_enable(sdata, true, true); + if (err < 0) + return err; + } + } + + if (cdata->sensors_enabled & ST_LSM6DS3H_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + disable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3h_common_resume); +#endif /* CONFIG_PM */ + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3h core driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c.c b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c.c new file mode 100644 index 000000000000..691761c6f5b9 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c.c @@ -0,0 +1,176 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3h i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lsm6ds3h.h" + +static int st_lsm6ds3h_i2c_read(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int st_lsm6ds3h_i2c_write(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + int err = 0; + u8 send[8]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, &msg, 1); + + return err; +} + +static const struct st_lsm6ds3h_transfer_function st_lsm6ds3h_tf_i2c = { + .write = st_lsm6ds3h_i2c_write, + .read = st_lsm6ds3h_i2c_read, +}; + +static int st_lsm6ds3h_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct lsm6ds3h_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + i2c_set_clientdata(client, cdata); + + cdata->tf = &st_lsm6ds3h_tf_i2c; + + err = st_lsm6ds3h_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6ds3h_i2c_remove(struct i2c_client *client) +{ + struct lsm6ds3h_data *cdata = i2c_get_clientdata(client); + + st_lsm6ds3h_common_remove(cdata, client->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_lsm6ds3h_suspend(struct device *dev) +{ + struct lsm6ds3h_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6ds3h_common_suspend(cdata); +} + +static int __maybe_unused st_lsm6ds3h_resume(struct device *dev) +{ + struct lsm6ds3h_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6ds3h_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6ds3h_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3h_suspend, st_lsm6ds3h_resume) +}; + +#define ST_LSM6DS3H_PM_OPS (&st_lsm6ds3h_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DS3H_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id st_lsm6ds3h_id_table[] = { + { LSM6DS3H_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6ds3h_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id lsm6ds3h_of_match[] = { + { + .compatible = "st,lsm6ds3h", + .data = LSM6DS3H_DEV_NAME, + }, + {} +}; +MODULE_DEVICE_TABLE(of, lsm6ds3h_of_match); +#else /* CONFIG_OF */ +#define lsm6ds3h_of_match NULL +#endif /* CONFIG_OF */ + +static struct i2c_driver st_lsm6ds3h_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6ds3h-i2c", + .pm = ST_LSM6DS3H_PM_OPS, + .of_match_table = of_match_ptr(lsm6ds3h_of_match), + }, + .probe = st_lsm6ds3h_i2c_probe, + .remove = st_lsm6ds3h_i2c_remove, + .id_table = st_lsm6ds3h_id_table, +}; +module_i2c_driver(st_lsm6ds3h_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3h i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c_master.c b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c_master.c new file mode 100644 index 000000000000..3f2f31251e58 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_i2c_master.c @@ -0,0 +1,1709 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3h i2c master driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_lsm6ds3h.h" + +#define EXT0_INDEX 0 + +#define ST_LSM6DS3H_ODR_LIST_NUM 4 +#define ST_LSM6DS3H_SENSOR_HUB_OP_TIMEOUT 5 +#define ST_LSM6DS3H_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DS3H_EN_BIT 0x01 +#define ST_LSM6DS3H_DIS_BIT 0x00 +#define ST_LSM6DS3H_SLV0_ADDR_ADDR 0x02 +#define ST_LSM6DS3H_SLV1_ADDR_ADDR 0x05 +#define ST_LSM6DS3H_SLV2_ADDR_ADDR 0x08 +#define ST_LSM6DS3H_SLV0_OUT_ADDR 0x2e +#define ST_LSM6DS3H_INTER_PULLUP_ADDR 0x1a +#define ST_LSM6DS3H_INTER_PULLUP_MASK 0x08 +#define ST_LSM6DS3H_FUNC_MAX_RATE_ADDR 0x18 +#define ST_LSM6DS3H_FUNC_MAX_RATE_MASK 0x02 +#define ST_LSM6DS3H_DATAWRITE_SLV0 0x0e +#define ST_LSM6DS3H_SLVX_READ 0x01 + +/* External sensors configuration */ +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6ds3h_sensor_data *sdata); + +#define ST_LSM6DS3H_EXT0_ADDR 0x1e +#define ST_LSM6DS3H_EXT0_ADDR2 0x1c +#define ST_LSM6DS3H_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DS3H_EXT0_WAI_VALUE 0x3d +#define ST_LSM6DS3H_EXT0_RESET_ADDR 0x21 +#define ST_LSM6DS3H_EXT0_RESET_MASK 0x04 +#define ST_LSM6DS3H_EXT0_FULLSCALE_ADDR 0x21 +#define ST_LSM6DS3H_EXT0_FULLSCALE_MASK 0x60 +#define ST_LSM6DS3H_EXT0_FULLSCALE_VALUE 0x02 +#define ST_LSM6DS3H_EXT0_ODR_ADDR 0x20 +#define ST_LSM6DS3H_EXT0_ODR_MASK 0x1c +#define ST_LSM6DS3H_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3H_EXT0_ODR0_VALUE 0x04 +#define ST_LSM6DS3H_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3H_EXT0_ODR1_VALUE 0x05 +#define ST_LSM6DS3H_EXT0_ODR2_HZ 40 +#define ST_LSM6DS3H_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3H_EXT0_ODR3_HZ 80 +#define ST_LSM6DS3H_EXT0_ODR3_VALUE 0x07 +#define ST_LSM6DS3H_EXT0_PW_ADDR 0x22 +#define ST_LSM6DS3H_EXT0_PW_MASK 0x03 +#define ST_LSM6DS3H_EXT0_PW_OFF 0x02 +#define ST_LSM6DS3H_EXT0_PW_ON 0x00 +#define ST_LSM6DS3H_EXT0_GAIN_VALUE 438 +#define ST_LSM6DS3H_EXT0_OUT_X_L_ADDR 0x28 +#define ST_LSM6DS3H_EXT0_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DS3H_EXT0_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DS3H_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3H_EXT0_BDU_ADDR 0x24 +#define ST_LSM6DS3H_EXT0_BDU_MASK 0x40 +#define ST_LSM6DS3H_EXT0_STD 0 +#define ST_LSM6DS3H_EXT0_BOOT_FUNCTION (&lis3mdl_initialization) +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN 2281 +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX 6843 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN_Z 228 +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX_Z 2281 +#define ST_LSM6DS3H_SELFTEST_ADDR1 0x20 +#define ST_LSM6DS3H_SELFTEST_ADDR2 0x21 +#define ST_LSM6DS3H_SELFTEST_ADDR3 0x22 +#define ST_LSM6DS3H_SELFTEST_ADDR1_VALUE 0x1c +#define ST_LSM6DS3H_SELFTEST_ADDR2_VALUE 0x40 +#define ST_LSM6DS3H_SELFTEST_ADDR3_VALUE 0x00 +#define ST_LSM6DS3H_SELFTEST_ENABLE 0x1d +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct lsm6ds3h_sensor_data *sdata); + +#define ST_LSM6DS3H_EXT0_ADDR 0x0c +#define ST_LSM6DS3H_EXT0_ADDR2 0x0d +#define ST_LSM6DS3H_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3H_EXT0_WAI_VALUE 0x05 +#define ST_LSM6DS3H_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3H_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3H_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3H_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3H_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3H_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3H_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3H_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3H_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3H_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3H_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3H_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3H_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3H_EXT0_PW_ADDR ST_LSM6DS3H_EXT0_ODR_ADDR +#define ST_LSM6DS3H_EXT0_PW_MASK ST_LSM6DS3H_EXT0_ODR_MASK +#define ST_LSM6DS3H_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3H_EXT0_PW_ON ST_LSM6DS3H_EXT0_ODR0_VALUE +#define ST_LSM6DS3H_EXT0_GAIN_VALUE 6000 +#define ST_LSM6DS3H_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3H_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3H_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3H_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3H_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3H_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3H_EXT0_STD 0 +#define ST_LSM6DS3H_EXT0_BOOT_FUNCTION (&akm09911_initialization) +#define ST_LSM6DS3H_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN (-30) +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX 30 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN_Z (-400) +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX_Z (-50) +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6ds3h_sensor_data *sdata); + +#define ST_LSM6DS3H_EXT0_ADDR 0x0c +#define ST_LSM6DS3H_EXT0_ADDR2 0x0d +#define ST_LSM6DS3H_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3H_EXT0_WAI_VALUE 0x04 +#define ST_LSM6DS3H_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3H_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3H_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3H_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3H_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3H_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3H_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3H_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3H_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3H_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3H_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3H_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3H_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3H_EXT0_PW_ADDR ST_LSM6DS3H_EXT0_ODR_ADDR +#define ST_LSM6DS3H_EXT0_PW_MASK ST_LSM6DS3H_EXT0_ODR_MASK +#define ST_LSM6DS3H_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3H_EXT0_PW_ON ST_LSM6DS3H_EXT0_ODR0_VALUE +#define ST_LSM6DS3H_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DS3H_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3H_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3H_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3H_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3H_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3H_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3H_EXT0_STD 0 +#define ST_LSM6DS3H_EXT0_BOOT_FUNCTION (&akm09912_initialization) +#define ST_LSM6DS3H_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN (-200) +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX 200 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN_Z (-1600) +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX_Z (-400) +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09916 +#define ST_LSM6DS3H_EXT0_ADDR 0x0c +#define ST_LSM6DS3H_EXT0_ADDR2 0x0c +#define ST_LSM6DS3H_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3H_EXT0_WAI_VALUE 0x09 +#define ST_LSM6DS3H_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3H_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3H_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3H_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3H_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3H_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3H_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3H_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3H_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3H_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3H_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3H_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3H_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3H_EXT0_PW_ADDR ST_LSM6DS3H_EXT0_ODR_ADDR +#define ST_LSM6DS3H_EXT0_PW_MASK ST_LSM6DS3H_EXT0_ODR_MASK +#define ST_LSM6DS3H_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3H_EXT0_PW_ON ST_LSM6DS3H_EXT0_ODR0_VALUE +#define ST_LSM6DS3H_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DS3H_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3H_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3H_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3H_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3H_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3H_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3H_EXT0_STD 0 +#define ST_LSM6DS3H_EXT0_BOOT_FUNCTION NULL +#define ST_LSM6DS3H_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN (-200) +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX 200 +#define ST_LSM6DS3H_SELFTEST_EXT0_MIN_Z (-1000) +#define ST_LSM6DS3H_SELFTEST_EXT0_MAX_Z (-200) +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09916 */ + + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct lsm6ds3h_sensor_data *sdata); + +#define ST_LSM6DS3H_EXT0_ADDR 0x5d +#define ST_LSM6DS3H_EXT0_ADDR2 0x5c +#define ST_LSM6DS3H_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DS3H_EXT0_WAI_VALUE 0xb1 +#define ST_LSM6DS3H_EXT0_RESET_ADDR 0x11 +#define ST_LSM6DS3H_EXT0_RESET_MASK 0x80 +#define ST_LSM6DS3H_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3H_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3H_EXT0_ODR_ADDR 0x10 +#define ST_LSM6DS3H_EXT0_ODR_MASK 0x70 +#define ST_LSM6DS3H_EXT0_ODR0_HZ 1 +#define ST_LSM6DS3H_EXT0_ODR0_VALUE 0x01 +#define ST_LSM6DS3H_EXT0_ODR1_HZ 10 +#define ST_LSM6DS3H_EXT0_ODR1_VALUE 0x02 +#define ST_LSM6DS3H_EXT0_ODR2_HZ 25 +#define ST_LSM6DS3H_EXT0_ODR2_VALUE 0x03 +#define ST_LSM6DS3H_EXT0_ODR3_HZ 50 +#define ST_LSM6DS3H_EXT0_ODR3_VALUE 0x04 +#define ST_LSM6DS3H_EXT0_PW_ADDR ST_LSM6DS3H_EXT0_ODR_ADDR +#define ST_LSM6DS3H_EXT0_PW_MASK ST_LSM6DS3H_EXT0_ODR_MASK +#define ST_LSM6DS3H_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3H_EXT0_PW_ON ST_LSM6DS3H_EXT0_ODR0_VALUE +#define ST_LSM6DS3H_EXT0_GAIN_VALUE 244 +#define ST_LSM6DS3H_EXT0_OUT_P_L_ADDR 0x28 +#define ST_LSM6DS3H_EXT0_OUT_T_L_ADDR 0x2b +#define ST_LSM6DS3H_EXT0_READ_DATA_LEN 5 +#define ST_LSM6DS3H_EXT0_BDU_ADDR 0x10 +#define ST_LSM6DS3H_EXT0_BDU_MASK 0x02 +#define ST_LSM6DS3H_EXT0_STD 0 +#define ST_LSM6DS3H_EXT0_BOOT_FUNCTION (&lps22hb_initialization) +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB */ + +/* SENSORS SUFFIX NAMES */ +#define ST_LSM6DS3H_EXT0_SUFFIX_NAME "magn" +#define ST_LSM6DS3H_EXT1_SUFFIX_NAME "press" + +#if defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911) +#define ST_LSM6DS3H_EXT0_HAS_SELFTEST 1 +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_MAGN */ + +#if defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911) +#define ST_LSM6DS3H_EXT0_IS_AKM 1 +#define ST_LSM6DS3H_SELFTEST_STATUS_REG 0x10 +#define ST_LSM6DS3H_SELFTEST_ADDR 0x31 +#define ST_LSM6DS3H_SELFTEST_ENABLE 0x10 +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM0099xx */ + + +struct st_lsm6ds3h_i2c_master_odr_reg { + unsigned int hz; + u8 value; +}; + +struct st_lsm6ds3h_i2c_master_odr_table { + u8 addr; + u8 mask; + struct st_lsm6ds3h_i2c_master_odr_reg odr_avl[ST_LSM6DS3H_ODR_LIST_NUM]; +}; + +static int st_lsm6ds3h_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask); + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB +static const struct iio_chan_spec st_lsm6ds3h_ext0_ch[] = { + ST_LSM6DS3H_LSM_CHANNELS(IIO_PRESSURE, 0, 0, IIO_NO_MOD, IIO_LE, + 24, 24, ST_LSM6DS3H_EXT0_OUT_P_L_ADDR, 'u'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_TEMP, 0, 1, IIO_NO_MOD, IIO_LE, + 16, 16, ST_LSM6DS3H_EXT0_OUT_T_L_ADDR, 's'), + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_PRESSURE), + IIO_CHAN_SOFT_TIMESTAMP(2) +}; +#else /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB */ +static const struct iio_chan_spec st_lsm6ds3h_ext0_ch[] = { + ST_LSM6DS3H_LSM_CHANNELS(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3H_EXT0_OUT_X_L_ADDR, 's'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3H_EXT0_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3H_LSM_CHANNELS(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3H_EXT0_OUT_Z_L_ADDR, 's'), + ST_LSM6DS3H_FLUSH_CHANNEL(IIO_MAGN), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB */ + +static int st_lsm6ds3h_i2c_master_set_odr(struct lsm6ds3h_sensor_data *sdata, + unsigned int odr, bool force); + +static int st_lsm6ds3h_i2c_master_write(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock); +static int st_lsm6ds3h_i2c_master_read(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset); + +#ifdef ST_LSM6DS3H_EXT0_HAS_SELFTEST +static ssize_t st_lsm6ds3h_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_lsm6ds3h_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_lsm6ds3h_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +#endif /* ST_LSM6DS3H_EXT0_HAS_SELFTEST */ + +static ssize_t st_lsm6ds3h_i2c_master_sysfs_sampling_frequency_avail( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%d %d %d %d\n", 13, 26, 52, 104); +} + +static ssize_t st_lsm6ds3h_i2c_master_sysfs_get_sampling_frequency( + struct device *dev, struct device_attribute *attr, char *buf) +{ + struct lsm6ds3h_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_lsm6ds3h_i2c_master_sysfs_set_sampling_frequency( + struct device *dev, struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + mutex_lock(&sdata->cdata->odr_lock); + + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_lsm6ds3h_i2c_master_set_odr(sdata, odr, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_lsm6ds3h_i2c_master_sysfs_get_sampling_frequency, + st_lsm6ds3h_i2c_master_sysfs_set_sampling_frequency); + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL( + st_lsm6ds3h_i2c_master_sysfs_sampling_frequency_avail); + +static ST_LSM6DS3H_HWFIFO_ENABLED(); +static ST_LSM6DS3H_HWFIFO_WATERMARK(); +static ST_LSM6DS3H_HWFIFO_WATERMARK_MIN(); +static ST_LSM6DS3H_HWFIFO_WATERMARK_MAX(); +static ST_LSM6DS3H_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6ds3h_get_module_id, NULL, 0); + +#ifdef ST_LSM6DS3H_EXT0_HAS_SELFTEST +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_lsm6ds3h_i2c_master_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_lsm6ds3h_i2c_master_sysfs_get_selftest_status, + st_lsm6ds3h_i2c_master_sysfs_start_selftest, 0); +#endif /* ST_LSM6DS3H_EXT0_HAS_SELFTEST */ + +static struct attribute *st_lsm6ds3h_ext0_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef ST_LSM6DS3H_EXT0_HAS_SELFTEST + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, +#endif /* ST_LSM6DS3H_EXT0_HAS_SELFTEST */ + + NULL, +}; + +static const struct attribute_group st_lsm6ds3h_ext0_attribute_group = { + .attrs = st_lsm6ds3h_ext0_attributes, +}; + +static const struct iio_info st_lsm6ds3h_ext0_info = { + .attrs = &st_lsm6ds3h_ext0_attribute_group, + .read_raw = &st_lsm6ds3h_i2c_master_read_raw, +}; + +struct st_lsm6ds3h_iio_info_data { + char suffix_name[20]; + struct iio_info *info; + struct iio_chan_spec *channels; + int num_channels; +}; + +struct st_lsm6ds3h_reg { + u8 addr; + u8 mask; + u8 def_value; +}; + +struct st_lsm6ds3h_power_reg { + u8 addr; + u8 mask; + u8 off_value; + u8 on_value; + bool isodr; +}; + +struct st_lsm6ds3h_custom_function { + int (*boot_initialization)(struct lsm6ds3h_sensor_data *sdata); +}; + +static struct st_lsm6ds3h_exs_list { + struct st_lsm6ds3h_reg wai; + struct st_lsm6ds3h_reg reset; + struct st_lsm6ds3h_reg fullscale; + struct st_lsm6ds3h_i2c_master_odr_table odr; + struct st_lsm6ds3h_power_reg power; + u8 fullscale_value; + u8 samples_to_discard; + u8 read_data_len; + u8 num_data_channels; + bool available; + unsigned int gain; + u8 i2c_addr; + struct st_lsm6ds3h_iio_info_data data; + struct st_lsm6ds3h_custom_function cf; +} st_lsm6ds3h_exs_list[] = { + { + .wai = { + .addr = ST_LSM6DS3H_EXT0_WAI_ADDR, + .def_value = ST_LSM6DS3H_EXT0_WAI_VALUE, + }, + .reset = { + .addr = ST_LSM6DS3H_EXT0_RESET_ADDR, + .mask = ST_LSM6DS3H_EXT0_RESET_MASK, + }, + .fullscale = { + .addr = ST_LSM6DS3H_EXT0_FULLSCALE_ADDR, + .mask = ST_LSM6DS3H_EXT0_FULLSCALE_MASK, + .def_value = ST_LSM6DS3H_EXT0_FULLSCALE_VALUE, + }, + .odr = { + .addr = ST_LSM6DS3H_EXT0_ODR_ADDR, + .mask = ST_LSM6DS3H_EXT0_ODR_MASK, + .odr_avl = { + { + .hz = ST_LSM6DS3H_EXT0_ODR0_HZ, + .value = ST_LSM6DS3H_EXT0_ODR0_VALUE, + }, + { + .hz = ST_LSM6DS3H_EXT0_ODR1_HZ, + .value = ST_LSM6DS3H_EXT0_ODR1_VALUE, + }, + { + .hz = ST_LSM6DS3H_EXT0_ODR2_HZ, + .value = ST_LSM6DS3H_EXT0_ODR2_VALUE, + }, + { + .hz = ST_LSM6DS3H_EXT0_ODR3_HZ, + .value = ST_LSM6DS3H_EXT0_ODR3_VALUE, + }, + }, + }, + .power = { + .addr = ST_LSM6DS3H_EXT0_PW_ADDR, + .mask = ST_LSM6DS3H_EXT0_PW_MASK, + .off_value = ST_LSM6DS3H_EXT0_PW_OFF, + .on_value = ST_LSM6DS3H_EXT0_PW_ON, + }, + .samples_to_discard = ST_LSM6DS3H_EXT0_STD, + .read_data_len = ST_LSM6DS3H_EXT0_READ_DATA_LEN, + .num_data_channels = 3, + .available = false, + .gain = ST_LSM6DS3H_EXT0_GAIN_VALUE, + .i2c_addr = ST_LSM6DS3H_EXT0_ADDR, + .data = { + .suffix_name = ST_LSM6DS3H_EXT0_SUFFIX_NAME, + .info = (struct iio_info *)&st_lsm6ds3h_ext0_info, + .channels = (struct iio_chan_spec *)&st_lsm6ds3h_ext0_ch, + .num_channels = ARRAY_SIZE(st_lsm6ds3h_ext0_ch), + }, + .cf.boot_initialization = ST_LSM6DS3H_EXT0_BOOT_FUNCTION, + } +}; + +static inline void st_lsm6ds3h_master_wait_completed(struct lsm6ds3h_data *cdata) +{ + msleep((1000U / cdata->trigger_odr) + 2); +} + +static int st_lsm6ds3h_i2c_master_read(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset) +{ + int err; + u8 slave_conf[3]; + + slave_conf[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1) | + ST_LSM6DS3H_SLVX_READ; + slave_conf[1] = reg_addr; + slave_conf[2] = (len & 0x07); + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + + if (en_sensor_hub) { + err = st_lsm6ds3h_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + + st_lsm6ds3h_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_LSM6DS3H_SLV0_OUT_ADDR + + offset, len & 0x07, data, true); + if (err < 0) + goto i2c_master_read_unlock_mutex; + +#ifdef ST_LSM6DS3H_EXT0_IS_AKM + if (read_status_end) { + slave_conf[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + slave_conf[1] = ST_LSM6DS3H_EXT0_DATA_STATUS; + slave_conf[2] = 0x01; + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } +#endif /* ST_LSM6DS3H_EXT0_IS_AKM */ + + if (en_sensor_hub) { + err = st_lsm6ds3h_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + +i2c_master_read_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len & 0x07; +} + +static int st_lsm6ds3h_i2c_master_write(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock) +{ + int err, i = 0; + u8 slave0_conf[2]; + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + while (i < len) { + slave0_conf[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1); + slave0_conf[1] = reg_addr + i; + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + slave0_conf[0] = data[i]; + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_DATAWRITE_SLV0, + slave0_conf, 1); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + if (en_sensor_hub) { + err = st_lsm6ds3h_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + st_lsm6ds3h_master_wait_completed(cdata); + + if (en_sensor_hub) { + err = st_lsm6ds3h_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + i++; + } + + slave0_conf[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1); + slave0_conf[1] = st_lsm6ds3h_exs_list[EXT0_INDEX].wai.addr; + + st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + +i2c_master_write_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len; +} + +static int st_lsm6ds3h_i2c_master_write_data_with_mask( + struct lsm6ds3h_data *cdata, u8 reg_addr, u8 mask, u8 data) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + mutex_lock(&cdata->i2c_transfer_lock); + disable_irq(cdata->irq); + + err = st_lsm6ds3h_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + err = st_lsm6ds3h_i2c_master_read(cdata, reg_addr, 1, + &old_data, false, false, true, + st_lsm6ds3h_exs_list[0].read_data_len); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data != old_data) + err = st_lsm6ds3h_i2c_master_write(cdata, reg_addr, + 1, &new_data, false, false); + + st_lsm6ds3h_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + + return err; +} + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6ds3h_sensor_data *sdata) +{ + + return st_lsm6ds3h_i2c_master_write_data_with_mask( + sdata->cdata, + ST_LSM6DS3H_EXT0_BDU_ADDR, + ST_LSM6DS3H_EXT0_BDU_MASK, ST_LSM6DS3H_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct lsm6ds3h_sensor_data *sdata) +{ + int err; u8 data[ST_LSM6DS3H_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, + ST_LSM6DS3H_EXT0_SENSITIVITY_ADDR, + ST_LSM6DS3H_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_lsm6ds3h_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0]) * 1000) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1]) * 1000) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2]) * 1000) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6ds3h_sensor_data *sdata) +{ + int err; u8 data[ST_LSM6DS3H_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, + ST_LSM6DS3H_EXT0_SENSITIVITY_ADDR, + ST_LSM6DS3H_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_lsm6ds3h_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0] - 128) * 500) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1] - 128) * 500) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2] - 128) * 500) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct lsm6ds3h_sensor_data *sdata) +{ + + return st_lsm6ds3h_i2c_master_write_data_with_mask( + sdata->cdata, + ST_LSM6DS3H_EXT0_BDU_ADDR, + ST_LSM6DS3H_EXT0_BDU_MASK, ST_LSM6DS3H_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LPS22HB */ + +#ifdef ST_LSM6DS3H_EXT0_HAS_SELFTEST +static ssize_t st_lsm6ds3h_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "absolute\n"); +} + +static ssize_t st_lsm6ds3h_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + result = sdata->cdata->ext0_selftest_status; + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_LSM6DS3H_SELFTEST_NA_MS; + else if (result < 0) + message = ST_LSM6DS3H_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_LSM6DS3H_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_lsm6ds3h_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + int x_selftest = 0, y_selftest = 0, z_selftest = 0; + u8 outdata[8], reg_addr, reg_status = 0, temp_reg_status; +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL + int i, x = 0, y = 0, z = 0; + u8 reg_status2 = 0, reg_status3 = 0; + u8 reg_addr2, reg_addr3, temp_reg_status2, temp_reg_status3; +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ +#ifdef ST_LSM6DS3H_EXT0_IS_AKM + u8 temp, sh_config[3], timeout = 0; +#endif /* ST_LSM6DS3H_EXT0_IS_AKM */ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + sdata->cdata->ext0_selftest_status = 0; + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + if (strncmp(buf, "absolute", size - 2) != 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = st_lsm6ds3h_enable_sensor_hub(sdata->cdata, true, ST_MASK_ID_EXT0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL + reg_addr = ST_LSM6DS3H_SELFTEST_ADDR1; + temp_reg_status = ST_LSM6DS3H_SELFTEST_ADDR1_VALUE; + reg_addr2 = ST_LSM6DS3H_SELFTEST_ADDR2; + temp_reg_status2 = ST_LSM6DS3H_SELFTEST_ADDR2_VALUE; + reg_addr3 = ST_LSM6DS3H_SELFTEST_ADDR3; + temp_reg_status3 = ST_LSM6DS3H_SELFTEST_ADDR3_VALUE; +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ + +#ifdef ST_LSM6DS3H_EXT0_IS_AKM + reg_addr = ST_LSM6DS3H_SELFTEST_ADDR; + temp_reg_status = ST_LSM6DS3H_SELFTEST_ENABLE; +#endif /* ST_LSM6DS3H_EXT0_IS_AKM */ + + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, reg_addr, 1, + ®_status, false, true, false, + st_lsm6ds3h_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + +#ifdef ST_LSM6DS3H_EXT0_IS_AKM + /* SLAVE 1 is disabled for a while, dummy write to wai reg */ + sh_config[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6ds3h_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; + + /* SLAVE 2 is disabled for a while, dummy read of wai reg */ + sh_config[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6ds3h_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_SLV2_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; +#endif /* ST_LSM6DS3H_EXT0_IS_AKM */ + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, reg_addr2, 1, + ®_status2, false, true, false, + st_lsm6ds3h_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, reg_addr3, 1, + ®_status3, false, true, false, + st_lsm6ds3h_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto disable_sensor_hub; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr2, 1, + &temp_reg_status2, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr3, 1, + &temp_reg_status3, false, true); + if (err < 0) + goto restore_status_reg2; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_lsm6ds3h_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 10; + y += ((s16)*(u16 *)&outdata[2]) / 10; + z += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + temp_reg_status = ST_LSM6DS3H_SELFTEST_ENABLE; + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto restore_status_reg3; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_lsm6ds3h_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 10; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 10; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); + if (err < 0) + goto restore_status_reg3; + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); + if (err < 0) + goto restore_status_reg2; + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6ds3h_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + if ((abs(x_selftest - x) < ST_LSM6DS3H_SELFTEST_EXT0_MIN) || + (abs(x_selftest - x) > ST_LSM6DS3H_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(y_selftest - y) < ST_LSM6DS3H_SELFTEST_EXT0_MIN) || + (abs(y_selftest - y) > ST_LSM6DS3H_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(z_selftest - z) < ST_LSM6DS3H_SELFTEST_EXT0_MIN_Z) || + (abs(z_selftest - z) > ST_LSM6DS3H_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ + +#ifdef ST_LSM6DS3H_EXT0_IS_AKM + do { + msleep(1000U / sdata->cdata->trigger_odr); + + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, + ST_LSM6DS3H_SELFTEST_STATUS_REG, 1, + &temp, false, true, false, 1); + if (err < 0) + goto restore_status_reg; + + timeout++; + } while (((temp & 0x01) == 0) && (timeout < 5)); + + if (timeout >= 5) { + err = -EINVAL; + goto restore_status_reg; + } + + err = st_lsm6ds3h_i2c_master_read(sdata->cdata, + st_lsm6ds3h_exs_list[0].data.channels[0].address, + st_lsm6ds3h_exs_list[0].read_data_len, + outdata, false, true, true, 1); + if (err < 0) + goto restore_status_reg; + +#ifdef ST_LSM6DS3H_EXT0_IS_AKM + /* SLAVE 2 recovering */ + sh_config[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6ds3h_exs_list[0].data.channels[0].address; + sh_config[2] = st_lsm6ds3h_exs_list[0].read_data_len; + + err = st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto restore_status_reg; +#endif /* ST_LSM6DS3H_EXT0_IS_AKM */ + + err = st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6ds3h_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + x_selftest = ((s16)*(u16 *)&outdata[0]); + y_selftest = ((s16)*(u16 *)&outdata[2]); + z_selftest = ((s16)*(u16 *)&outdata[4]); + +#if defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM09911) + x_selftest *= sdata->c_gain[0]; + y_selftest *= sdata->c_gain[1]; + z_selftest *= sdata->c_gain[2]; + + x_selftest /= 10000; + y_selftest /= 10000; + z_selftest /= 10000; +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_AKM0991X */ + + if ((x_selftest < ST_LSM6DS3H_SELFTEST_EXT0_MIN) || + (x_selftest > ST_LSM6DS3H_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((y_selftest < ST_LSM6DS3H_SELFTEST_EXT0_MIN) || + (y_selftest > ST_LSM6DS3H_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((z_selftest < ST_LSM6DS3H_SELFTEST_EXT0_MIN_Z) || + (z_selftest > ST_LSM6DS3H_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* ST_LSM6DS3H_EXT0_IS_AKM */ + + sdata->cdata->ext0_selftest_status = 1; + + mutex_unlock(&sdata->cdata->odr_lock); + + return size; + +#ifdef CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL +restore_status_reg3: + st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); +restore_status_reg2: + st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); +#endif /* CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS3MDL */ +restore_status_reg: + st_lsm6ds3h_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); +disable_sensor_hub: + st_lsm6ds3h_enable_sensor_hub(sdata->cdata, false, ST_MASK_ID_EXT0); + mutex_unlock(&sdata->cdata->odr_lock); + return err; +} +#endif /* ST_LSM6DS3H_EXT0_HAS_SELFTEST */ + + +static int st_lsm6ds3h_i2c_master_set_odr(struct lsm6ds3h_sensor_data *sdata, + unsigned int odr, bool force) +{ + int i, err, err2; + u8 value, mask, addr; + bool scan_odr = true; + unsigned int current_odr = sdata->cdata->v_odr[sdata->sindex]; + unsigned int current_hw_odr = sdata->cdata->hw_odr[sdata->sindex]; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + if (scan_odr) { + switch (odr) { + case 13: + case 26: + case 52: + case 104: + break; + default: + return -EINVAL; + } + + for (i = 0; i < ST_LSM6DS3H_ODR_LIST_NUM; i++) { + if (st_lsm6ds3h_exs_list[0].odr.odr_avl[i].hz >= odr) + break; + } + if (i == ST_LSM6DS3H_ODR_LIST_NUM) + i--; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = odr; + return 0; + } + } + + addr = st_lsm6ds3h_exs_list[0].odr.addr; + mask = st_lsm6ds3h_exs_list[0].odr.mask; + value = st_lsm6ds3h_exs_list[0].odr.odr_avl[i].value; + } else { + if (st_lsm6ds3h_exs_list[0].power.isodr) { + addr = st_lsm6ds3h_exs_list[0].power.addr; + mask = st_lsm6ds3h_exs_list[0].power.mask; + value = st_lsm6ds3h_exs_list[0].power.off_value; + } else + goto skip_i2c_write; + } + + sdata->cdata->samples_to_discard[ST_MASK_ID_EXT0] = + st_lsm6ds3h_exs_list[0].samples_to_discard; + + err = st_lsm6ds3h_i2c_master_write_data_with_mask(sdata->cdata, + addr, mask, value); + if (err < 0) + return err; + +skip_i2c_write: + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = odr; + + if (!force) { + sdata->cdata->v_odr[sdata->sindex] = odr; + + err = st_lsm6ds3h_enable_sensor_hub(sdata->cdata, + true, ST_MASK_ID_EXT0); + if (err < 0) { + sdata->cdata->hw_odr[sdata->sindex] = current_hw_odr; + sdata->cdata->v_odr[sdata->sindex] = current_odr; + do { + err2 = st_lsm6ds3h_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + msleep(200); + } while (err2 < 0); + + return err; + } + } + + return 0; +} + +static int st_lsm6ds3h_i2c_master_set_enable( + struct lsm6ds3h_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + /* If odr != power this part should enable/disable sensor */ + if (!st_lsm6ds3h_exs_list[0].power.isodr) { + if (enable) + reg_value = st_lsm6ds3h_exs_list[0].power.on_value; + else + reg_value = st_lsm6ds3h_exs_list[0].power.off_value; + + err = st_lsm6ds3h_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_exs_list[0].power.addr, + st_lsm6ds3h_exs_list[0].power.mask, + reg_value); + if (err < 0) + return err; + } + + err = st_lsm6ds3h_enable_sensor_hub(sdata->cdata, + enable, ST_MASK_ID_EXT0); + if (err < 0) + return err; + + err = st_lsm6ds3h_i2c_master_set_odr(sdata, + enable ? sdata->cdata->v_odr[sdata->sindex] : 0, true); + if (err < 0) + goto disable_sensorhub; + + if (buffer) { + err = st_lsm6ds3h_set_drdy_irq(sdata, enable); + if (err < 0) + goto restore_odr; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; + +restore_odr: + st_lsm6ds3h_i2c_master_set_odr(sdata, + enable ? 0 : sdata->cdata->v_odr[sdata->sindex], true); +disable_sensorhub: + st_lsm6ds3h_enable_sensor_hub(sdata->cdata, !enable, ST_MASK_ID_EXT0); + + return err; +} + +static int st_lsm6ds3h_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + int err, ch_num_byte = ch->scan_type.storagebits >> 3; + u8 outdata[4]; + + if (ch_num_byte > ARRAY_SIZE(outdata)) + return -ENOMEM; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3h_i2c_master_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + st_lsm6ds3h_master_wait_completed(sdata->cdata); + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + ch_num_byte, outdata, true); + if (err < 0) { + st_lsm6ds3h_i2c_master_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + err = st_lsm6ds3h_i2c_master_set_enable(sdata, false, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (ch_num_byte > 2) + *val = (s32)get_unaligned_le32(outdata); + else + *val = (s16)get_unaligned_le16(outdata); + + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[ch->scan_index]; + + if (ch->type == IIO_TEMP) { + *val = 1; + *val2 = 0; + return IIO_VAL_INT; + } + + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6ds3h_i2c_master_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) + return -EBUSY; +#endif /* CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION */ + + return 0; +} + +static int st_lsm6ds3h_i2c_master_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) && + (indio_dev->buffer->length < 2 * ST_LSM6DS3H_MAX_FIFO_LENGHT)) + return -EINVAL; + + sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); + if (!sdata->buffer_data) + return -ENOMEM; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3h_i2c_master_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + return 0; +} + +static int st_lsm6ds3h_i2c_master_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3h_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6ds3h_i2c_master_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + kfree(sdata->buffer_data); + + return err < 0 ? err : 0; +} + +static const struct iio_trigger_ops st_lsm6ds3h_i2c_master_trigger_ops = { + .set_trigger_state = &st_lsm6ds3h_trig_set_state, +}; + +int st_lsm6ds3h_i2c_master_allocate_trigger(struct lsm6ds3h_data *cdata) +{ + int err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->trig[ST_MASK_ID_EXT0]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + return -ENOMEM; + } + + iio_trigger_set_drvdata(cdata->trig[ST_MASK_ID_EXT0], + cdata->indio_dev[ST_MASK_ID_EXT0]); + cdata->trig[ST_MASK_ID_EXT0]->ops = &st_lsm6ds3h_i2c_master_trigger_ops; + cdata->trig[ST_MASK_ID_EXT0]->dev.parent = cdata->dev; + + err = iio_trigger_register(cdata->trig[ST_MASK_ID_EXT0]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + goto deallocate_trigger; + } + + cdata->indio_dev[ST_MASK_ID_EXT0]->trig = cdata->trig[ST_MASK_ID_EXT0]; + + return 0; + +deallocate_trigger: + iio_trigger_free(cdata->trig[ST_MASK_ID_EXT0]); + return err; +} + +static void st_lsm6ds3h_i2c_master_deallocate_trigger(struct lsm6ds3h_data *cdata) +{ + iio_trigger_unregister(cdata->trig[ST_MASK_ID_EXT0]); +} + +static const struct iio_buffer_setup_ops st_lsm6ds3h_i2c_master_buffer_setup_ops = { + .preenable = &st_lsm6ds3h_i2c_master_buffer_preenable, + .postenable = &st_lsm6ds3h_i2c_master_buffer_postenable, + .postdisable = &st_lsm6ds3h_i2c_master_buffer_postdisable, +}; + +static inline irqreturn_t st_lsm6ds3h_i2c_master_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +static int st_lsm6ds3h_i2c_master_allocate_buffer(struct lsm6ds3h_data *cdata) +{ + return iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_EXT0], + &st_lsm6ds3h_i2c_master_handler_empty, NULL, + &st_lsm6ds3h_i2c_master_buffer_setup_ops); +} + +static void st_lsm6ds3h_i2c_master_deallocate_buffer(struct lsm6ds3h_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_EXT0]); +} + +static int st_lsm6ds3h_i2c_master_send_sensor_hub_parameters( + struct lsm6ds3h_sensor_data *sdata) +{ + int err; + u8 sh_config[3]; + + /* SLAVE 0 is used by write */ + sh_config[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1); + sh_config[1] = st_lsm6ds3h_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x01 | 0x20; + + err = st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + /* SLAVE 1 is used to read output data */ + sh_config[0] = (st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_LSM6DS3H_EN_BIT; + sh_config[1] = st_lsm6ds3h_exs_list[0].data.channels[0].address; + sh_config[2] = st_lsm6ds3h_exs_list[0].read_data_len; + + err = st_lsm6ds3h_write_embedded_registers(sdata->cdata, + ST_LSM6DS3H_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6ds3h_i2c_master_init_sensor(struct lsm6ds3h_sensor_data *sdata) +{ + int err, ext_num = 0; + + err = st_lsm6ds3h_i2c_master_send_sensor_hub_parameters(sdata); + if (err < 0) + return err; + + sdata->c_gain[0] = st_lsm6ds3h_exs_list[ext_num].gain; + sdata->c_gain[1] = st_lsm6ds3h_exs_list[ext_num].gain; + sdata->c_gain[2] = st_lsm6ds3h_exs_list[ext_num].gain; + + if ((st_lsm6ds3h_exs_list[ext_num].power.addr == + st_lsm6ds3h_exs_list[ext_num].odr.addr) && + (st_lsm6ds3h_exs_list[ext_num].power.mask == + st_lsm6ds3h_exs_list[ext_num].odr.mask)) + st_lsm6ds3h_exs_list[ext_num].power.isodr = true; + else + st_lsm6ds3h_exs_list[ext_num].power.isodr = false; + + err = st_lsm6ds3h_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_exs_list[ext_num].reset.addr, + st_lsm6ds3h_exs_list[ext_num].reset.mask, + ST_LSM6DS3H_EN_BIT); + if (err < 0) + return err; + + usleep_range(200, 1000); + + if (st_lsm6ds3h_exs_list[ext_num].fullscale.addr > 0) { + err = st_lsm6ds3h_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6ds3h_exs_list[ext_num].fullscale.addr, + st_lsm6ds3h_exs_list[ext_num].fullscale.mask, + st_lsm6ds3h_exs_list[ext_num].fullscale.def_value); + if (err < 0) + return err; + } + + if (st_lsm6ds3h_exs_list[0].cf.boot_initialization != NULL) { + err = st_lsm6ds3h_exs_list[0].cf.boot_initialization(sdata); + if (err < 0) + return err; + } + + err = st_lsm6ds3h_i2c_master_set_enable(sdata, false, false); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6ds3h_i2c_master_allocate_device(struct lsm6ds3h_data *cdata) +{ + int err; + struct lsm6ds3h_sensor_data *sdata_ext; + + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + + sdata_ext->num_data_channels = + st_lsm6ds3h_exs_list[0].num_data_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->name = kasprintf(GFP_KERNEL, + "%s_%s", cdata->name, + st_lsm6ds3h_exs_list[0].data.suffix_name); + + cdata->indio_dev[ST_MASK_ID_EXT0]->info = + st_lsm6ds3h_exs_list[0].data.info; + cdata->indio_dev[ST_MASK_ID_EXT0]->channels = + st_lsm6ds3h_exs_list[0].data.channels; + cdata->indio_dev[ST_MASK_ID_EXT0]->num_channels = + st_lsm6ds3h_exs_list[0].data.num_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->modes = INDIO_DIRECT_MODE; + + sdata_ext->data_out_reg = ST_LSM6DS3H_SLV0_OUT_ADDR; + + err = st_lsm6ds3h_i2c_master_init_sensor(sdata_ext); + if (err < 0) + return err; + + err = st_lsm6ds3h_i2c_master_allocate_buffer(cdata); + if (err < 0) + return err; + + err = st_lsm6ds3h_i2c_master_allocate_trigger(cdata); + if (err < 0) + goto iio_deallocate_buffer; + + err = iio_device_register(cdata->indio_dev[ST_MASK_ID_EXT0]); + if (err < 0) + goto iio_deallocate_trigger; + + return 0; + +iio_deallocate_trigger: + st_lsm6ds3h_i2c_master_deallocate_trigger(cdata); +iio_deallocate_buffer: + st_lsm6ds3h_i2c_master_deallocate_buffer(cdata); + + return err; +} + +static void st_lsm6ds3h_i2c_master_deallocate_device(struct lsm6ds3h_data *cdata) +{ + iio_device_unregister(cdata->indio_dev[ST_MASK_ID_EXT0]); + st_lsm6ds3h_i2c_master_deallocate_trigger(cdata); + st_lsm6ds3h_i2c_master_deallocate_buffer(cdata); +} + +int st_lsm6ds3h_i2c_master_probe(struct lsm6ds3h_data *cdata) +{ + int err, i; + u8 sh_config[3]; + u8 wai, i2c_address; + struct lsm6ds3h_sensor_data *sdata_ext; + + mutex_init(&cdata->i2c_transfer_lock); + cdata->v_odr[ST_MASK_ID_EXT0] = 13; + cdata->ext0_available = false; + cdata->ext0_selftest_status = false; + +#ifdef CONFIG_ST_LSM6DS3H_ENABLE_INTERNAL_PULLUP + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_INTER_PULLUP_ADDR, + ST_LSM6DS3H_INTER_PULLUP_MASK, + ST_LSM6DS3H_EN_BIT, true); + if (err < 0) + return err; +#endif /* CONFIG_ST_LSM6DS3H_ENABLE_INTERNAL_PULLUP */ + + err = st_lsm6ds3h_write_data_with_mask(cdata, + ST_LSM6DS3H_FUNC_MAX_RATE_ADDR, + ST_LSM6DS3H_FUNC_MAX_RATE_MASK, 1, true); + if (err < 0) + return err; + + cdata->indio_dev[ST_MASK_ID_EXT0] = devm_iio_device_alloc(cdata->dev, + sizeof(*sdata_ext)); + if (!cdata->indio_dev[ST_MASK_ID_EXT0]) + return -ENOMEM; + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + sdata_ext->cdata = cdata; + sdata_ext->sindex = ST_MASK_ID_EXT0; + cdata->samples_to_discard_2[ST_MASK_ID_EXT0] = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].sip = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = 0; + + for (i = 0; i < 2; i++) { + if (i == 0) + i2c_address = ST_LSM6DS3H_EXT0_ADDR; + else + i2c_address = ST_LSM6DS3H_EXT0_ADDR2; + + /* to check if sensor is available use SLAVE0 first time */ + sh_config[0] = (i2c_address << 1) | 0x01; + sh_config[1] = st_lsm6ds3h_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x01; + + err = st_lsm6ds3h_write_embedded_registers(cdata, + ST_LSM6DS3H_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + err = st_lsm6ds3h_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + msleep(100); + + st_lsm6ds3h_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_LSM6DS3H_SLV0_OUT_ADDR, + 1, &wai, true); + if (err < 0) { + err = st_lsm6ds3h_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + continue; + } + + err = st_lsm6ds3h_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + st_lsm6ds3h_exs_list[EXT0_INDEX].i2c_addr = i2c_address; + break; + } + if (i == 2) + goto ext0_sensor_not_available; + + /* after wai check SLAVE0 is used for write, SLAVE1 for async read + and SLAVE2 to read sensor output data */ + + if (wai != st_lsm6ds3h_exs_list[EXT0_INDEX].wai.def_value) { + dev_err(cdata->dev, "wai value of external sensor 0 mismatch\n"); + return err; + } + + err = st_lsm6ds3h_i2c_master_allocate_device(cdata); + if (err < 0) + return err; + + cdata->ext0_available = true; + + return 0; + +ext0_sensor_not_available: + dev_err(cdata->dev, "external sensor 0 not available\n"); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3h_i2c_master_probe); + +int st_lsm6ds3h_i2c_master_exit(struct lsm6ds3h_data *cdata) +{ + if (cdata->ext0_available) + st_lsm6ds3h_i2c_master_deallocate_device(cdata); + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3h_i2c_master_exit); diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_spi.c b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_spi.c new file mode 100644 index 000000000000..0f0aa238c447 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_spi.c @@ -0,0 +1,194 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3h spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lsm6ds3h.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lsm6ds3h_spi_read(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int st_lsm6ds3h_spi_write(struct lsm6ds3h_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_LSM6DS3H_RX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static const struct st_lsm6ds3h_transfer_function st_lsm6ds3h_tf_spi = { + .write = st_lsm6ds3h_spi_write, + .read = st_lsm6ds3h_spi_read, +}; + +static int st_lsm6ds3h_spi_probe(struct spi_device *spi) +{ + int err; + struct lsm6ds3h_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + spi_set_drvdata(spi, cdata); + + cdata->tf = &st_lsm6ds3h_tf_spi; + + err = st_lsm6ds3h_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6ds3h_spi_remove(struct spi_device *spi) +{ + struct lsm6ds3h_data *cdata = spi_get_drvdata(spi); + + st_lsm6ds3h_common_remove(cdata, spi->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_lsm6ds3h_suspend(struct device *dev) +{ + struct lsm6ds3h_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6ds3h_common_suspend(cdata); +} + +static int __maybe_unused st_lsm6ds3h_resume(struct device *dev) +{ + struct lsm6ds3h_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6ds3h_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6ds3h_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3h_suspend, st_lsm6ds3h_resume) +}; + +#define ST_LSM6DS3H_PM_OPS (&st_lsm6ds3h_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DS3H_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id st_lsm6ds3h_id_table[] = { + { LSM6DS3H_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6ds3h_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id lsm6ds3h_of_match[] = { + { + .compatible = "st,lsm6ds3h", + .data = LSM6DS3H_DEV_NAME, + }, + {} +}; +MODULE_DEVICE_TABLE(of, lsm6ds3h_of_match); +#else /* CONFIG_OF */ +#define lsm6ds3h_of_match NULL +#endif /* CONFIG_OF */ + +static struct spi_driver st_lsm6ds3h_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6ds3h-spi", + .pm = ST_LSM6DS3H_PM_OPS, + .of_match_table = of_match_ptr(lsm6ds3h_of_match), + }, + .probe = st_lsm6ds3h_spi_probe, + .remove = st_lsm6ds3h_spi_remove, + .id_table = st_lsm6ds3h_id_table, +}; +module_spi_driver(st_lsm6ds3h_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3h spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_trigger.c b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_trigger.c new file mode 100644 index 000000000000..6b6868faf7fb --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6ds3h/st_lsm6ds3h_trigger.c @@ -0,0 +1,207 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6ds3h trigger driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6ds3h.h" + +#define ST_LSM6DS3H_DIS_BIT 0x00 +#define ST_LSM6DS3H_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DS3H_FIFO_DATA_AVL_ADDR 0x3b +#define ST_LSM6DS3H_ACCEL_DATA_AVL_ADDR 0x1e + +#define ST_LSM6DS3H_ACCEL_DATA_AVL 0x01 +#define ST_LSM6DS3H_GYRO_DATA_AVL 0x02 +#define ST_LSM6DS3H_SRC_STEP_DETECTOR_DATA_AVL 0x10 +#define ST_LSM6DS3H_SRC_SIGN_MOTION_DATA_AVL 0x40 +#define ST_LSM6DS3H_SRC_TILT_DATA_AVL 0x20 +#define ST_LSM6DS3H_SRC_STEP_COUNTER_DATA_AVL 0x80 +#define ST_LSM6DS3H_SRC_STEP_COUNTER_DATA_OVR 0x08 +#define ST_LSM6DS3H_FIFO_DATA_AVL 0x80 +#define ST_LSM6DS3H_FIFO_DATA_OVR 0x40 + + +static irqreturn_t lsm6ds3h_irq_management(int irq, void *private) +{ + int err; + bool push; + bool force_read_accel = false; + struct lsm6ds3h_data *cdata = private; + u8 src_accel_gyro = 0, src_dig_func = 0; + + cdata->timestamp = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + (BIT(ST_MASK_ID_ACCEL) | BIT(ST_MASK_ID_GYRO) | + BIT(ST_MASK_ID_EXT0))) { + err = cdata->tf->read(cdata, ST_LSM6DS3H_ACCEL_DATA_AVL_ADDR, + 1, &src_accel_gyro, true); + if (err < 0) + goto read_fifo_status; + + if (src_accel_gyro & ST_LSM6DS3H_ACCEL_DATA_AVL) { +#ifdef CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) + & BIT(ST_MASK_ID_EXT0)) { + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples++; + force_read_accel = true; + + if ((cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = 0; + } else { + push = false; + } + + lsm6ds3h_read_output_data(cdata, ST_MASK_ID_EXT0, push); + } +#endif /* CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT */ + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + BIT(ST_MASK_ID_ACCEL)) { + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples++; + + if ((cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = 0; + } else { + push = false; + } + + lsm6ds3h_read_output_data(cdata, ST_MASK_ID_ACCEL, push); + } else { + if (force_read_accel) + lsm6ds3h_read_output_data(cdata, ST_MASK_ID_ACCEL, false); + } + + } + + if (src_accel_gyro & ST_LSM6DS3H_GYRO_DATA_AVL) { + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & BIT(ST_MASK_ID_GYRO)) + lsm6ds3h_read_output_data(cdata, ST_MASK_ID_GYRO, true); + } + } + +read_fifo_status: + if (cdata->sensors_use_fifo) + st_lsm6ds3h_read_fifo(cdata, false); + + err = cdata->tf->read(cdata, ST_LSM6DS3H_SRC_FUNC_ADDR, + 1, &src_dig_func, true); + if (err < 0) + goto exit_irq; + + if ((src_dig_func & ST_LSM6DS3H_SRC_STEP_DETECTOR_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_STEP_DETECTOR))) { + st_lsm6ds3h_push_data_with_timestamp(cdata, + ST_MASK_ID_STEP_DETECTOR, NULL, cdata->timestamp); + } + + if ((src_dig_func & ST_LSM6DS3H_SRC_SIGN_MOTION_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_SIGN_MOTION))) { + iio_push_event(cdata->indio_dev[ST_MASK_ID_SIGN_MOTION], + IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->timestamp); + } + + if (src_dig_func & ST_LSM6DS3H_SRC_STEP_COUNTER_DATA_OVR) + cdata->num_steps += (1 << 16); + + if (src_dig_func & ST_LSM6DS3H_SRC_STEP_COUNTER_DATA_AVL) + iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_STEP_COUNTER]); + + if ((src_dig_func & ST_LSM6DS3H_SRC_TILT_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_TILT))) { + st_lsm6ds3h_push_data_with_timestamp(cdata, + ST_MASK_ID_TILT, NULL, cdata->timestamp); + } + +exit_irq: + return IRQ_HANDLED; +} + +int st_lsm6ds3h_allocate_triggers(struct lsm6ds3h_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[i] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->trig[i]) { + dev_err(cdata->dev, + "failed to allocate iio trigger.\n"); + err = -ENOMEM; + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]); + cdata->trig[i]->ops = trigger_ops; + cdata->trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, NULL, lsm6ds3h_irq_management, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_trigger_register(cdata->trig[n]); + if (err < 0) { + dev_err(cdata->dev, + "failed to register iio trigger.\n"); + goto free_irq; + } + cdata->indio_dev[n]->trig = cdata->trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->trig[i]); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3h_allocate_triggers); + +void st_lsm6ds3h_deallocate_triggers(struct lsm6ds3h_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_trigger_unregister(cdata->trig[i]); +} +EXPORT_SYMBOL(st_lsm6ds3h_deallocate_triggers); diff --git a/drivers/iio/stm/imu/st_lsm6dsm/Kconfig b/drivers/iio/stm/imu/st_lsm6dsm/Kconfig new file mode 100644 index 000000000000..eded4ef39173 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/Kconfig @@ -0,0 +1,96 @@ +# SPDX-License-Identifier: GPL-2.0-only + +# +# st-lsm6dsm drivers for STMicroelectronics combo sensor +# + +menuconfig ST_LSM6DSM_IIO + tristate "STMicroelectronics LSM6DSM/LSM6DSL sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select ST_LSM6DSM_I2C_IIO if (I2C) + select ST_LSM6DSM_SPI_IIO if (SPI) + help + This driver supports LSM6DSM and LSM6DSL sensors. + It is a gyroscope/accelerometer combo device. + This driver can be built as a module. The module will be called + st-lsm6dsm. + +if ST_LSM6DSM_IIO + +config ST_LSM6DSM_I2C_IIO + tristate + depends on ST_LSM6DSM_IIO + depends on I2C + +config ST_LSM6DSM_SPI_IIO + tristate + depends on ST_LSM6DSM_IIO + depends on SPI + +config ST_LSM6DSM_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on ST_LSM6DSM_IIO + range 0 4096 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +config ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND + bool "Keep Step counter on during suspend" + depends on ST_LSM6DSM_IIO + default n + help + During suspend step counter is kept on if enabled. Only interrupt + is disabled. + +menuconfig ST_LSM6DSM_IIO_MASTER_SUPPORT + bool "I2C master controller" + depends on I2C && ST_LSM6DSM_IIO + default n + help + Added support for I2C master controller. Only one slave sensor is + supported. + +if ST_LSM6DSM_IIO_MASTER_SUPPORT + +config ST_LSM6DSM_ENABLE_INTERNAL_PULLUP + bool "Enabled internals pull-up resistors" + default y + +choice + prompt "External sensor 0" + default ST_LSM6DSM_IIO_EXT0_LIS3MDL + help + Choose the external sensor 0 connected to LSM6DS3. + +config ST_LSM6DSM_IIO_EXT0_LIS3MDL + bool "LIS3MDL" +config ST_LSM6DSM_IIO_EXT0_AKM09911 + bool "AKM09911" +config ST_LSM6DSM_IIO_EXT0_AKM09912 + bool "AKM09912" +config ST_LSM6DSM_IIO_EXT0_AKM09916 + bool "AKM09916" +config ST_LSM6DSM_IIO_EXT0_LPS22HB + bool "LPS22HB" +config ST_LSM6DSM_IIO_EXT0_LIS2MDL + bool "LIS2MDL" +endchoice + +endif + +config ST_LSM6DSM_XL_DATA_INJECTION + bool "Enable XL data injection support" + depends on ST_LSM6DSM_IIO + default n + help + This option enables the accelerometer data injection + support. The device functions may so use an injected + pattern instead of taking the real sensor data. + +endif #ST_LSM6DSM_IIO diff --git a/drivers/iio/stm/imu/st_lsm6dsm/Makefile b/drivers/iio/stm/imu/st_lsm6dsm/Makefile new file mode 100644 index 000000000000..c6f5ce1592bc --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/Makefile @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Makefile for STMicroelectronics LSM6DSM sensor. +# + +obj-$(CONFIG_ST_LSM6DSM_IIO) += st_lsm6dsm.o +st_lsm6dsm-objs := st_lsm6dsm_core.o +obj-$(CONFIG_ST_LSM6DSM_I2C_IIO) += st_lsm6dsm_i2c.o +obj-$(CONFIG_ST_LSM6DSM_SPI_IIO) += st_lsm6dsm_spi.o + +st_lsm6dsm-$(CONFIG_IIO_BUFFER) += st_lsm6dsm_buffer.o +st_lsm6dsm-$(CONFIG_IIO_TRIGGER) += st_lsm6dsm_trigger.o +st_lsm6dsm-$(CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT) += st_lsm6dsm_i2c_master.o diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm.h b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm.h new file mode 100644 index 000000000000..1d764fadf392 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm.h @@ -0,0 +1,371 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lsm6dsm driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSM_H +#define ST_LSM6DSM_H + +#include +#include + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT +#include +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + +#define LSM6DSM_DEV_NAME "lsm6dsm" +#define LSM6DSL_DEV_NAME "lsm6dsl" + +enum st_mask_id { + ST_MASK_ID_ACCEL = 0, + ST_MASK_ID_GYRO, + ST_MASK_ID_SIGN_MOTION, + ST_MASK_ID_STEP_COUNTER, + ST_MASK_ID_STEP_DETECTOR, + ST_MASK_ID_TILT, + ST_MASK_ID_WTILT, + ST_MASK_ID_TAP, + ST_MASK_ID_TAP_TAP, + ST_MASK_ID_EXT0, + ST_MASK_ID_HW_PEDOMETER, + ST_MASK_ID_SENSOR_HUB, + ST_MASK_ID_DIGITAL_FUNC, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP, +}; + +#define ST_INDIO_DEV_NUM 9 + +#define ST_LSM6DSM_TX_MAX_LENGTH 12 +#define ST_LSM6DSM_RX_MAX_LENGTH 4097 + +#define ST_LSM6DSM_BYTE_FOR_CHANNEL 2 +#define ST_LSM6DSM_BYTE_FOR_WRIST_TILT 1 +#define ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE 6 + +#define ST_LSM6DSM_MAX_FIFO_SIZE 4096 +#define ST_LSM6DSM_MAX_FIFO_THRESHOLD 546 +#define ST_LSM6DSM_MAX_FIFO_LENGHT (ST_LSM6DSM_MAX_FIFO_SIZE / \ + ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE) + +#define ST_LSM6DSM_SELFTEST_NA_MS "na" +#define ST_LSM6DSM_SELFTEST_FAIL_MS "fail" +#define ST_LSM6DSM_SELFTEST_PASS_MS "pass" + +#define ST_LSM6DSM_WAKE_UP_SENSORS (BIT(ST_MASK_ID_SIGN_MOTION) | \ + BIT(ST_MASK_ID_TILT) | BIT(ST_MASK_ID_WTILT)) + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT +#define ST_LSM6DSM_NUM_CLIENTS 1 +#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ +#define ST_LSM6DSM_NUM_CLIENTS 0 +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + +#define ST_LSM6DSM_LSM_CHANNELS(device_type, modif, index, mod, \ + endian, sbits, rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +extern const struct iio_event_spec lsm6dsm_fifo_flush_event; + +#define ST_LSM6DSM_FLUSH_CHANNEL(device_type) \ +{ \ + .type = device_type, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &lsm6dsm_fifo_flush_event,\ + .num_event_specs = 1, \ +} + +#define ST_LSM6DSM_HWFIFO_ENABLED() \ + IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \ + st_lsm6dsm_sysfs_get_hwfifo_enabled,\ + st_lsm6dsm_sysfs_set_hwfifo_enabled, 0); + +#define ST_LSM6DSM_HWFIFO_WATERMARK() \ + IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \ + st_lsm6dsm_sysfs_get_hwfifo_watermark,\ + st_lsm6dsm_sysfs_set_hwfifo_watermark, 0); + +#define ST_LSM6DSM_HWFIFO_WATERMARK_MIN() \ + IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \ + st_lsm6dsm_sysfs_get_hwfifo_watermark_min, NULL, 0); + +#define ST_LSM6DSM_HWFIFO_WATERMARK_MAX() \ + IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \ + st_lsm6dsm_sysfs_get_hwfifo_watermark_max, NULL, 0); + +#define ST_LSM6DSM_HWFIFO_FLUSH() \ + IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \ + st_lsm6dsm_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +struct st_lsm6dsm_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[ST_LSM6DSM_RX_MAX_LENGTH]; + u8 tx_buf[ST_LSM6DSM_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct lsm6dsm_out_decimation { + short decimator; + short num_samples; +}; + +struct lsm6dsm_fifo_output { + u8 sip; + int64_t deltatime; + int64_t deltatime_default; + int64_t timestamp; + int64_t timestamp_p; + short decimator; + short num_samples; + bool initialized; +}; + +/* struct lsm6dsm_data - common data for i2c or spi driver instance + * @name: pointer to the device name (i2c name or spi modalias). + * @enable_digfunc_mask: mask used to enable/disable hw digital functions. + * @enable_pedometer_mask: mask used to enable/disable hw pedometer function. + * @enable_sensorhub_mask: mask used to enable/disable sensor-hub feature. + * @irq_enable_fifo_mask: mask used to enable/disable fifo irq. + * @irq_enable_accel_ext_mask: mask used to enable/disable accel irq. + * @hw_odr: physical sensor odr expressed in Hz. + * @v_odr: requested sensor odr by userspace expressed in Hz. + * @hwfifo_enabled: is hwfifo enabled? + * @hwfifo_decimator: hwfifo decimator factor. + * @hwfifo_watermark: hwfifo watermark value. + * @samples_to_discard: samples to discard due to ODR switch. + * @nofifo_decimation: output status when fifo is disabled. + * @fifo_output: output status when fifo is enabled. + * @sensors_enabled: sensors enabled mask. + * @sensors_use_fifo: sensors use fifo mask. + * @accel_odr_dependency: odr dependency: accel, sensor-hub, dig-func. + * @accel_on: accel is going to be enabled during fifo odr switch? + * @magn_on: magn is going to be enabled during fifo odr switch? + * @odr_lock: mutex to avoid race condition during odr switch. + * @reset_steps: do I need to reset number of steps? + * @fifo_data: fifo data. + * @gyro_selftest_status: gyroscope selftest result. + * @accel_selftest_status: accelerometer selftest result. + * @irq: irq number. + * @timestamp: timestamp value from boot process. + * @module_id: identify iio devices of the same sensor module. + */ +struct lsm6dsm_data { + const char *name; + + u16 enable_digfunc_mask; + u16 enable_pedometer_mask; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + u16 enable_sensorhub_mask; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + u16 irq_enable_fifo_mask; + u16 irq_enable_accel_ext_mask; + + unsigned int hw_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int trigger_odr; + + bool hwfifo_enabled[ST_INDIO_DEV_NUM + 1]; + u8 hwfifo_decimator[ST_INDIO_DEV_NUM + 1]; + u16 hwfifo_watermark[ST_INDIO_DEV_NUM + 1]; + u16 fifo_watermark; + + u8 samples_to_discard[ST_INDIO_DEV_NUM + 1]; + u8 samples_to_discard_2[ST_INDIO_DEV_NUM + 1]; + struct lsm6dsm_out_decimation nofifo_decimation[ST_INDIO_DEV_NUM + 1]; + struct lsm6dsm_fifo_output fifo_output[ST_INDIO_DEV_NUM + 1]; + + u16 sensors_enabled; + u16 sensors_use_fifo; + u64 num_steps; + + int accel_odr_dependency[3]; + + bool accel_on; + bool magn_on; + enum fifo_mode fifo_status; + + struct mutex odr_lock; + + bool reset_steps; + + u8 *fifo_data; + u8 accel_last_push[6]; + u8 gyro_last_push[6]; + u8 ext0_last_push[6]; + int8_t gyro_selftest_status; + int8_t accel_selftest_status; + + u8 drdy_reg; + int irq; + + s64 timestamp; + int64_t fifo_enable_timestamp; + int64_t slower_counter; + uint8_t slower_id; + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + bool injection_mode; + s64 last_injection_timestamp; + u8 injection_odr; +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + struct work_struct data_work; + + struct device *dev; + struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + 1]; + struct iio_trigger *trig[ST_INDIO_DEV_NUM + 1]; + struct mutex bank_registers_lock; + struct mutex fifo_lock; + u32 module_id; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + bool ext0_available; + int8_t ext0_selftest_status; + struct mutex i2c_transfer_lock; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + const struct st_lsm6dsm_transfer_function *tf; + struct st_lsm6dsm_transfer_buffer tb; +}; + +struct st_lsm6dsm_transfer_function { + int (*write)(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); + int (*read)(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); +}; + +struct lsm6dsm_sensor_data { + struct lsm6dsm_data *cdata; + + unsigned int c_gain[3]; + + u8 num_data_channels; + u8 sindex; + u8 data_out_reg; +}; + +int st_lsm6dsm_write_data_with_mask(struct lsm6dsm_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock); + +int st_lsm6dsm_push_data_with_timestamp(struct lsm6dsm_data *cdata, + u8 index, u8 *data, int64_t timestamp); + +int st_lsm6dsm_common_probe(struct lsm6dsm_data *cdata, int irq); +void st_lsm6dsm_common_remove(struct lsm6dsm_data *cdata, int irq); + +int st_lsm6dsm_set_enable(struct lsm6dsm_sensor_data *sdata, bool enable, bool buffer); +int st_lsm6dsm_set_fifo_mode(struct lsm6dsm_data *cdata, enum fifo_mode fm); +int st_lsm6dsm_enable_sensor_hub(struct lsm6dsm_data *cdata, bool enable, + enum st_mask_id id); +int lsm6dsm_read_output_data(struct lsm6dsm_data *cdata, int sindex, bool push); +int st_lsm6dsm_set_drdy_irq(struct lsm6dsm_sensor_data *sdata, bool state); + +ssize_t st_lsm6dsm_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6dsm_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6dsm_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6dsm_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +ssize_t st_lsm6dsm_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +#ifdef CONFIG_IIO_BUFFER +int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata); +void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata); +int st_lsm6dsm_trig_set_state(struct iio_trigger *trig, bool state); +int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async); +#define ST_LSM6DSM_TRIGGER_SET_STATE (&st_lsm6dsm_trig_set_state) +#else /* CONFIG_IIO_BUFFER */ +static inline int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata) +{ + return 0; +} +static inline void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata) +{ +} +static inline int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async) +{ + return 0; +} +#define ST_LSM6DSM_TRIGGER_SET_STATE NULL +#endif /* CONFIG_IIO_BUFFER */ + +#ifdef CONFIG_IIO_TRIGGER +int st_lsm6dsm_allocate_triggers(struct lsm6dsm_data *cdata, + const struct iio_trigger_ops *trigger_ops); +void st_lsm6dsm_deallocate_triggers(struct lsm6dsm_data *cdata); +void st_lsm6dsm_flush_works(void); +#else /* CONFIG_IIO_TRIGGER */ +static inline int st_lsm6dsm_allocate_triggers(struct lsm6dsm_data *cdata, + const struct iio_trigger_ops *trigger_ops, int irq) +{ + return 0; +} +static inline void st_lsm6dsm_deallocate_triggers(struct lsm6dsm_data *cdata, + int irq) +{ + return; +} +static inline void st_lsm6dsm_flush_works(void) +{ + return; +} +#endif /* CONFIG_IIO_TRIGGER */ + +#ifdef CONFIG_PM +int st_lsm6dsm_common_suspend(struct lsm6dsm_data *cdata); +int st_lsm6dsm_common_resume(struct lsm6dsm_data *cdata); +#endif /* CONFIG_PM */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT +int st_lsm6dsm_write_embedded_registers(struct lsm6dsm_data *cdata, + u8 reg_addr, u8 *data, int len); +int st_lsm6dsm_i2c_master_probe(struct lsm6dsm_data *cdata); +int st_lsm6dsm_i2c_master_exit(struct lsm6dsm_data *cdata); +#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ +static inline int st_lsm6dsm_i2c_master_probe(struct lsm6dsm_data *cdata) +{ + return 0; +} +static inline int st_lsm6dsm_i2c_master_exit(struct lsm6dsm_data *cdata) +{ + return 0; +} +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + +#endif /* ST_LSM6DSM_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_buffer.c b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_buffer.c new file mode 100644 index 000000000000..bc5535f60b12 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_buffer.c @@ -0,0 +1,685 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6dsm buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_lsm6dsm.h" + +#define ST_LSM6DSM_FIFO_DIFF_L 0x3a +#define ST_LSM6DSM_FIFO_DIFF_MASK 0x07 +#define ST_LSM6DSM_FIFO_DATA_OUT_L 0x3e +#define ST_LSM6DSM_FIFO_DATA_OVR 0x40 +#define ST_LSM6DSM_FIFO_DATA_EMPTY 0x10 +#define ST_LSM6DSM_STEP_MASK_64BIT (0xFFFFFFFFFFFF0000) + +#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \ + (d) : (c)) : ((b == 0) ? \ + (c) : (d))) + +int st_lsm6dsm_push_data_with_timestamp(struct lsm6dsm_data *cdata, + u8 index, u8 *data, int64_t timestamp) +{ + int i, n = 0; + struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels; + uint16_t bfch, bfchs_out = 0, bfchs_in = 0; + struct lsm6dsm_sensor_data *sdata = iio_priv(cdata->indio_dev[index]); + u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)]; + + if (timestamp <= cdata->fifo_output[index].timestamp_p) + return -EINVAL; + + for (i = 0; i < sdata->num_data_channels; i++) { + bfch = chs[i].scan_type.storagebits >> 3; + + if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) { + memcpy(&buff[bfchs_out], &data[bfchs_in], bfch); + n++; + bfchs_out += bfch; + } + + bfchs_in += bfch; + } + + iio_push_to_buffers_with_timestamp(cdata->indio_dev[index], + buff, timestamp); + + cdata->fifo_output[index].timestamp_p = timestamp; + + return 0; +} + +static void st_lsm6dsm_parse_fifo_data(struct lsm6dsm_data *cdata, + u16 read_len, int64_t time_top, u16 num_pattern) +{ + int err; + u16 fifo_offset = 0; + u8 gyro_sip, accel_sip; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + u8 ext0_sip; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + while (fifo_offset < read_len) { + gyro_sip = cdata->fifo_output[ST_MASK_ID_GYRO].sip; + accel_sip = cdata->fifo_output[ST_MASK_ID_ACCEL].sip; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + do { + if (gyro_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_GYRO) + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top - + (num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_GYRO].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp += cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp -= cdata->fifo_output[ST_MASK_ID_GYRO].deltatime; + cdata->samples_to_discard[ST_MASK_ID_GYRO] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_GYRO] > 0) + cdata->samples_to_discard[ST_MASK_ID_GYRO]--; + else { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].num_samples >= cdata->fifo_output[ST_MASK_ID_GYRO].decimator) { + cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_GYRO)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_GYRO] == 0) { + err = st_lsm6dsm_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_GYRO].initialized = true; + + memcpy(cdata->gyro_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_GYRO]--; + + if (cdata->fifo_output[ST_MASK_ID_GYRO].initialized) { + err = st_lsm6dsm_push_data_with_timestamp( + cdata, ST_MASK_ID_GYRO, + cdata->gyro_last_push, + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE; + gyro_sip--; + } + + if (accel_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_ACCEL) + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top - + (num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp += cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp -= cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime; + cdata->samples_to_discard[ST_MASK_ID_ACCEL] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_ACCEL] > 0) + cdata->samples_to_discard[ST_MASK_ID_ACCEL]--; + else { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples >= cdata->fifo_output[ST_MASK_ID_ACCEL].decimator) { + cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] == 0) { + err = st_lsm6dsm_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].initialized = true; + + memcpy(cdata->accel_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_ACCEL]--; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].initialized) { + err = st_lsm6dsm_push_data_with_timestamp( + cdata, ST_MASK_ID_ACCEL, + cdata->accel_last_push, + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE; + accel_sip--; + } + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if (ext0_sip > 0) { + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp == 0) { + if (cdata->slower_id == ST_MASK_ID_EXT0) + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000; + else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top - + (num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000 - + (cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_EXT0].deltatime); + } else + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp += cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp > time_top) { + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp -= cdata->fifo_output[ST_MASK_ID_EXT0].deltatime; + cdata->samples_to_discard[ST_MASK_ID_EXT0] = 1; + } + + if (cdata->samples_to_discard[ST_MASK_ID_EXT0] > 0) + cdata->samples_to_discard[ST_MASK_ID_EXT0]--; + else { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples++; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].num_samples >= cdata->fifo_output[ST_MASK_ID_EXT0].decimator) { + cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = 0; + + if (cdata->sensors_enabled & BIT(ST_MASK_ID_EXT0)) { + if (cdata->samples_to_discard_2[ST_MASK_ID_EXT0] == 0) { + err = st_lsm6dsm_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + &cdata->fifo_data[fifo_offset], + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + + if (err >= 0) + cdata->fifo_output[ST_MASK_ID_EXT0].initialized = true; + + memcpy(cdata->ext0_last_push, &cdata->fifo_data[fifo_offset], 6); + } else { + cdata->samples_to_discard_2[ST_MASK_ID_EXT0]--; + + if (cdata->fifo_output[ST_MASK_ID_EXT0].initialized) { + err = st_lsm6dsm_push_data_with_timestamp( + cdata, ST_MASK_ID_EXT0, + cdata->ext0_last_push, + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp); + } + } + } + } + } + + fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE; + ext0_sip--; + } + + } while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0)); +#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } while ((accel_sip > 0) || (gyro_sip > 0)); +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } +} + +int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async) +{ + int err; + u8 fifo_status[2]; +#if (CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read; +#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */ + u16 read_len = 0, byte_in_pattern, num_pattern; + int64_t temp_counter = 0, timestamp_diff, slower_deltatime; + + err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DIFF_L, + 2, fifo_status, true); + if (err < 0) + return err; + + timestamp_diff = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if (fifo_status[1] & ST_LSM6DSM_FIFO_DATA_OVR) { + st_lsm6dsm_set_fifo_mode(cdata, BYPASS); + st_lsm6dsm_set_fifo_mode(cdata, CONTINUOS); + dev_err(cdata->dev, "data fifo overrun, failed to read it.\n"); + return -EINVAL; + } + + if (fifo_status[1] & ST_LSM6DSM_FIFO_DATA_EMPTY) + return 0; + + read_len = ((fifo_status[1] & ST_LSM6DSM_FIFO_DIFF_MASK) << 8) | fifo_status[0]; + read_len *= ST_LSM6DSM_BYTE_FOR_CHANNEL; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip + + cdata->fifo_output[ST_MASK_ID_EXT0].sip) * + ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE; +#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip + + cdata->fifo_output[ST_MASK_ID_GYRO].sip) * + ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + if (byte_in_pattern == 0) + return 0; + + num_pattern = read_len / byte_in_pattern; + + read_len = (read_len / byte_in_pattern) * byte_in_pattern; + if (read_len == 0) + return 0; + +#if (CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO == 0) + err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DATA_OUT_L, + read_len, cdata->fifo_data, true); + if (err < 0) + return err; +#else /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DATA_OUT_L, + data_to_read, + &cdata->fifo_data[read_len - data_remaining], true); + if (err < 0) + return err; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */ + + cdata->slower_id = MIN_ID(cdata->fifo_output[ST_MASK_ID_GYRO].sip, + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, + ST_MASK_ID_GYRO, ST_MASK_ID_ACCEL); +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + cdata->slower_id = MIN_ID(cdata->fifo_output[cdata->slower_id].sip, + cdata->fifo_output[ST_MASK_ID_EXT0].sip, + cdata->slower_id, ST_MASK_ID_EXT0); +#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */ + + temp_counter = cdata->slower_counter; + cdata->slower_counter += (read_len / byte_in_pattern) * cdata->fifo_output[cdata->slower_id].sip; + + if (async) + goto parse_fifo; + + if (temp_counter > 0) { + slower_deltatime = div64_s64(timestamp_diff - cdata->fifo_enable_timestamp, cdata->slower_counter); + + switch (cdata->slower_id) { + case ST_MASK_ID_ACCEL: + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_GYRO: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0) + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip); + + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = slower_deltatime; + break; + + case ST_MASK_ID_EXT0: + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0) + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0) + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime * + cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip); + + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = slower_deltatime; + break; + + default: + break; + } + } else { + cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default; + cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default; +#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */ + } + +parse_fifo: + st_lsm6dsm_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern); + + return 0; +} + +int lsm6dsm_read_output_data(struct lsm6dsm_data *cdata, int sindex, bool push) +{ + int err; + u8 data[6]; + struct iio_dev *indio_dev = cdata->indio_dev[sindex]; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + err = cdata->tf->read(cdata, sdata->data_out_reg, + ST_LSM6DSM_BYTE_FOR_CHANNEL * 3, data, true); + if (err < 0) + return err; + + if (push) + st_lsm6dsm_push_data_with_timestamp(cdata, sindex, + data, cdata->timestamp); + + return 0; +} +EXPORT_SYMBOL(lsm6dsm_read_output_data); + +static irqreturn_t st_lsm6dsm_outdata_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static irqreturn_t st_lsm6dsm_step_counter_trigger_handler(int irq, void *p) +{ + int err; + u8 steps_data[2]; + int64_t timestamp = 0; + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)]; + + if (!sdata->cdata->reset_steps) { + err = sdata->cdata->tf->read(sdata->cdata, + (u8)indio_dev->channels[0].address, + ST_LSM6DSM_BYTE_FOR_CHANNEL, + steps_data, true); + if (err < 0) + goto st_lsm6dsm_step_counter_done; + + sdata->cdata->num_steps = (sdata->cdata->num_steps & + ST_LSM6DSM_STEP_MASK_64BIT) + *((u16 *)steps_data); + timestamp = sdata->cdata->timestamp; + } else { + sdata->cdata->num_steps = 0; + timestamp = iio_get_time_ns(indio_dev); + sdata->cdata->reset_steps = false; + } + + memcpy(buff, (u8 *)&sdata->cdata->num_steps, sizeof(u64)); + iio_push_to_buffers_with_timestamp(indio_dev, buff, timestamp); + +st_lsm6dsm_step_counter_done: + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static irqreturn_t st_lsm6dsm_wrist_tilt_trigger_handler(int irq, void *p) +{ + int err; + u8 wrist_tilt_gesture; + int64_t timestamp; + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)]; + + err = sdata->cdata->tf->read(sdata->cdata, + (u8)indio_dev->channels[0].address, + ST_LSM6DSM_BYTE_FOR_WRIST_TILT, + &wrist_tilt_gesture, true); + if (err < 0) + goto st_lsm6dsm_wrist_tilt_done; + + buff[0] = wrist_tilt_gesture; + timestamp = sdata->cdata->timestamp; + + iio_push_to_buffers_with_timestamp(indio_dev, buff, timestamp); + +st_lsm6dsm_wrist_tilt_done: + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static inline irqreturn_t st_lsm6dsm_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int st_lsm6dsm_trig_set_state(struct iio_trigger *trig, bool state) +{ + return 0; +} + +static int st_lsm6dsm_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) { + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + case ST_MASK_ID_GYRO: + return -EBUSY; + + default: + break; + } + } +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + return 0; +} + +static int st_lsm6dsm_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[sdata->sindex]) && + (indio_dev->buffer->length < 2 * ST_LSM6DSM_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6dsm_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (sdata->sindex == ST_MASK_ID_STEP_COUNTER) + iio_trigger_poll_chained(sdata->cdata->trig[ST_MASK_ID_STEP_COUNTER]); + + return 0; +} + +static int st_lsm6dsm_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6dsm_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : 0; +} + +static const struct iio_buffer_setup_ops st_lsm6dsm_buffer_setup_ops = { + .preenable = &st_lsm6dsm_buffer_preenable, + .postenable = &st_lsm6dsm_buffer_postenable, + .postdisable = &st_lsm6dsm_buffer_postdisable, +}; + +int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata) +{ + int err; + struct lsm6dsm_sensor_data *sdata; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_ACCEL], + NULL, &st_lsm6dsm_outdata_trigger_handler, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + return err; + + sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_GYRO]); + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_GYRO], + NULL, &st_lsm6dsm_outdata_trigger_handler, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_accel; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION], + &st_lsm6dsm_handler_empty, NULL, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_gyro; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER], + NULL, + &st_lsm6dsm_step_counter_trigger_handler, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_sign_motion; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR], + &st_lsm6dsm_handler_empty, NULL, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_counter; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_TILT], + &st_lsm6dsm_handler_empty, NULL, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_detector; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_WTILT], + NULL, + &st_lsm6dsm_wrist_tilt_trigger_handler, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_tilt; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_TAP], + &st_lsm6dsm_handler_empty, NULL, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_wtilt; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_MASK_ID_TAP_TAP], + &st_lsm6dsm_handler_empty, NULL, + &st_lsm6dsm_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_tap; + + return 0; + +buffer_cleanup_tap: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP]); +buffer_cleanup_wtilt: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_WTILT]); +buffer_cleanup_tilt: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]); +buffer_cleanup_step_detector: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]); +buffer_cleanup_step_counter: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]); +buffer_cleanup_sign_motion: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]); +buffer_cleanup_gyro: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +buffer_cleanup_accel: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + return err; +} + +void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP_TAP]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_WTILT]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_core.c b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_core.c new file mode 100644 index 000000000000..409a50c59e6e --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_core.c @@ -0,0 +1,3497 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6dsm core driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "st_lsm6dsm.h" + +#define MS_TO_NS(msec) ((msec) * 1000 * 1000) + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif + +#define MIN_BNZ(a, b) (((a) < (b)) ? ((a == 0) ? \ + (b) : (a)) : ((b == 0) ? \ + (a) : (b))) + +/* COMMON VALUES FOR ACCEL-GYRO SENSORS */ +#define ST_LSM6DSM_DRDY_PULSE_CFG_G 0x0b +#define ST_LSM6DSM_WAI_ADDRESS 0x0f +#define ST_LSM6DSM_WAI_EXP 0x6a +#define ST_LSM6DSM_INT1_ADDR 0x0d +#define ST_LSM6DSM_INT2_ADDR 0x0e +#define ST_LSM6DSM_ACCEL_DRDY_IRQ_MASK 0x01 +#define ST_LSM6DSM_GYRO_DRDY_IRQ_MASK 0x02 +#define ST_LSM6DSM_MD1_ADDR 0x5e +#define ST_LSM6DSM_ODR_LIST_NUM 7 +#define ST_LSM6DSM_ODR_POWER_OFF_VAL 0x00 +#define ST_LSM6DSM_ODR_13HZ_VAL 0x01 +#define ST_LSM6DSM_ODR_26HZ_VAL 0x02 +#define ST_LSM6DSM_ODR_52HZ_VAL 0x03 +#define ST_LSM6DSM_ODR_104HZ_VAL 0x04 +#define ST_LSM6DSM_ODR_208HZ_VAL 0x05 +#define ST_LSM6DSM_ODR_416HZ_VAL 0x06 +#define ST_LSM6DSM_ODR_833HZ_VAL 0x07 +#define ST_LSM6DSM_FS_LIST_NUM 4 +#define ST_LSM6DSM_BDU_ADDR 0x12 +#define ST_LSM6DSM_BDU_MASK 0x40 +#define ST_LSM6DSM_EN_BIT 0x01 +#define ST_LSM6DSM_DIS_BIT 0x00 +#define ST_LSM6DSM_FUNC_EN_ADDR 0x19 +#define ST_LSM6DSM_FUNC_EN_MASK 0x04 +#define ST_LSM6DSM_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DSM_FUNC_CFG_ACCESS_MASK 0x01 +#define ST_LSM6DSM_FUNC_CFG_ACCESS_MASK2 0x04 +#define ST_LSM6DSM_FUNC_CFG_REG2_MASK 0x80 +#define ST_LSM6DSM_FUNC_CFG_START1_ADDR 0x62 +#define ST_LSM6DSM_FUNC_CFG_START2_ADDR 0x63 +#define ST_LSM6DSM_SENSORHUB_ADDR 0x1a +#define ST_LSM6DSM_SENSORHUB_MASK 0x01 +#define ST_LSM6DSM_SENSORHUB_TRIG_MASK 0x10 +#define ST_LSM6DSM_TRIG_INTERNAL 0x00 +#define ST_LSM6DSM_TRIG_EXTERNAL 0x01 +#define ST_LSM6DSM_SELFTEST_ADDR 0x14 +#define ST_LSM6DSM_SELFTEST_ACCEL_MASK 0x03 +#define ST_LSM6DSM_SELFTEST_GYRO_MASK 0x0c +#define ST_LSM6DSM_SELF_TEST_DISABLED_VAL 0x00 +#define ST_LSM6DSM_SELF_TEST_POS_SIGN_VAL 0x01 +#define ST_LSM6DSM_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 +#define ST_LSM6DSM_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 +#define ST_LSM6DSM_LIR_ADDR 0x58 +#define ST_LSM6DSM_LIR_MASK 0x01 +#define ST_LSM6DSM_INT_ENABLE_MASK 0x80 +#define ST_LSM6DSM_TIMER_EN_ADDR 0x19 +#define ST_LSM6DSM_TIMER_EN_MASK 0x20 +#define ST_LSM6DSM_PEDOMETER_EN_ADDR 0x19 +#define ST_LSM6DSM_PEDOMETER_EN_MASK 0x10 +#define ST_LSM6DSM_INT2_ON_INT1_ADDR 0x13 +#define ST_LSM6DSM_INT2_ON_INT1_MASK 0x20 +#define ST_LSM6DSM_MIN_DURATION_MS 1638 +#define ST_LSM6DSM_ROUNDING_ADDR 0x16 +#define ST_LSM6DSM_ROUNDING_MASK 0x04 +#define ST_LSM6DSM_FIFO_MODE_ADDR 0x0a +#define ST_LSM6DSM_FIFO_MODE_MASK 0x07 +#define ST_LSM6DSM_FIFO_MODE_BYPASS 0x00 +#define ST_LSM6DSM_FIFO_MODE_CONTINUOS 0x06 +#define ST_LSM6DSM_FIFO_THRESHOLD_IRQ_MASK 0x08 +#define ST_LSM6DSM_FIFO_ODR_MAX 0x40 +#define ST_LSM6DSM_FIFO_DECIMATOR_ADDR 0x08 +#define ST_LSM6DSM_FIFO_ACCEL_DECIMATOR_MASK 0x07 +#define ST_LSM6DSM_FIFO_GYRO_DECIMATOR_MASK 0x38 +#define ST_LSM6DSM_FIFO_DECIMATOR2_ADDR 0x09 +#define ST_LSM6DSM_FIFO_THR_L_ADDR 0x06 +#define ST_LSM6DSM_FIFO_THR_H_ADDR 0x07 +#define ST_LSM6DSM_FIFO_THR_MASK 0x07ff +#define ST_LSM6DSM_FIFO_THR_H_MASK 0x07 +#define ST_LSM6DSM_FIFO_THR_IRQ_MASK 0x08 +#define ST_LSM6DSM_RESET_ADDR 0x12 +#define ST_LSM6DSM_RESET_MASK 0x01 +#define ST_LSM6DSM_TEST_REG_ADDR 0x00 +#define ST_LSM6DSM_START_INJECT_XL_MASK 0x08 +#define ST_LSM6DSM_INJECT_XL_X_ADDR 0x06 +#define ST_LSM6DSM_SELFTEST_NA_MS "na" +#define ST_LSM6DSM_SELFTEST_FAIL_MS "fail" +#define ST_LSM6DSM_SELFTEST_PASS_MS "pass" + +/* CUSTOM VALUES FOR ACCEL SENSOR */ +#define ST_LSM6DSM_ACCEL_ODR_ADDR 0x10 +#define ST_LSM6DSM_ACCEL_ODR_MASK 0xf0 +#define ST_LSM6DSM_ACCEL_FS_ADDR 0x10 +#define ST_LSM6DSM_ACCEL_FS_MASK 0x0c +#define ST_LSM6DSM_ACCEL_FS_2G_VAL 0x00 +#define ST_LSM6DSM_ACCEL_FS_4G_VAL 0x02 +#define ST_LSM6DSM_ACCEL_FS_8G_VAL 0x03 +#define ST_LSM6DSM_ACCEL_FS_16G_VAL 0x01 +#define ST_LSM6DSM_ACCEL_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_LSM6DSM_ACCEL_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_LSM6DSM_ACCEL_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_LSM6DSM_ACCEL_FS_16G_GAIN IIO_G_TO_M_S_2(488000) +#define ST_LSM6DSM_ACCEL_OUT_X_L_ADDR 0x28 +#define ST_LSM6DSM_ACCEL_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DSM_ACCEL_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DSM_ACCEL_STD_52HZ 1 +#define ST_LSM6DSM_ACCEL_STD_104HZ 2 +#define ST_LSM6DSM_ACCEL_STD_208HZ 3 +#define ST_LSM6DSM_SELFTEST_ACCEL_ADDR 0x10 +#define ST_LSM6DSM_SELFTEST_ACCEL_REG_VALUE 0x40 +#define ST_LSM6DSM_SELFTEST_ACCEL_MIN 1492 +#define ST_LSM6DSM_SELFTEST_ACCEL_MAX 27868 + +/* CUSTOM VALUES FOR GYRO SENSOR */ +#define ST_LSM6DSM_GYRO_ODR_ADDR 0x11 +#define ST_LSM6DSM_GYRO_ODR_MASK 0xf0 +#define ST_LSM6DSM_GYRO_FS_ADDR 0x11 +#define ST_LSM6DSM_GYRO_FS_MASK 0x0c +#define ST_LSM6DSM_GYRO_FS_250_VAL 0x00 +#define ST_LSM6DSM_GYRO_FS_500_VAL 0x01 +#define ST_LSM6DSM_GYRO_FS_1000_VAL 0x02 +#define ST_LSM6DSM_GYRO_FS_2000_VAL 0x03 +#define ST_LSM6DSM_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_LSM6DSM_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_LSM6DSM_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_LSM6DSM_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_LSM6DSM_GYRO_OUT_X_L_ADDR 0x22 +#define ST_LSM6DSM_GYRO_OUT_Y_L_ADDR 0x24 +#define ST_LSM6DSM_GYRO_OUT_Z_L_ADDR 0x26 +#define ST_LSM6DSM_GYRO_STD_13HZ 2 +#define ST_LSM6DSM_GYRO_STD_52HZ 3 +#define ST_LSM6DSM_GYRO_STD_104HZ 5 +#define ST_LSM6DSM_GYRO_STD_208HZ 16 +#define ST_LSM6DSM_SELFTEST_GYRO_ADDR 0x11 +#define ST_LSM6DSM_SELFTEST_GYRO_REG_VALUE 0x4c +#define ST_LSM6DSM_SELFTEST_GYRO_MIN 2142 +#define ST_LSM6DSM_SELFTEST_GYRO_MAX 10000 + +/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */ +#define ST_LSM6DSM_SIGN_MOTION_EN_ADDR 0x19 +#define ST_LSM6DSM_SIGN_MOTION_EN_MASK 0x01 +#define ST_LSM6DSM_SIGN_MOTION_DRDY_IRQ_MASK 0x40 + +/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ +#define ST_LSM6DSM_STEP_DETECTOR_DRDY_IRQ_MASK 0x80 + +/* CUSTOM VALUES FOR STEP COUNTER SENSOR */ +#define ST_LSM6DSM_STEP_COUNTER_DRDY_IRQ_MASK 0x80 +#define ST_LSM6DSM_STEP_COUNTER_OUT_L_ADDR 0x4b +#define ST_LSM6DSM_STEP_COUNTER_RES_ADDR 0x19 +#define ST_LSM6DSM_STEP_COUNTER_RES_MASK 0x06 +#define ST_LSM6DSM_STEP_COUNTER_RES_ALL_EN 0x03 +#define ST_LSM6DSM_STEP_COUNTER_RES_FUNC_EN 0x02 +#define ST_LSM6DSM_STEP_COUNTER_DURATION_ADDR 0x15 +#define ST_LSM6DSM_STEP_COUNTER_THS_ADDR 0x0f +#define ST_LSM6DSM_STEP_COUNTER_THS_2G_VALUE (0x00 | 0x10) +#define ST_LSM6DSM_STEP_COUNTER_THS_4G_VALUE (0x80 | 0x08) + +/* CUSTOM VALUES FOR TILT SENSOR */ +#define ST_LSM6DSM_TILT_EN_ADDR 0x19 +#define ST_LSM6DSM_TILT_EN_MASK 0x08 +#define ST_LSM6DSM_TILT_DRDY_IRQ_MASK 0x02 + +/* CUSTOM VALUES FOR TAP AND TAP_TAP SENSOR */ +#define ST_LSM6DSM_TAP_EN_ADDR 0x58 +#define ST_LSM6DSM_TAP_EN_MASK GENMASK(3, 1) +#define ST_LSM6DSM_STAP_DRDY_IRQ_MASK 0x40 +#define ST_LSM6DSM_DTAP_DRDY_IRQ_MASK 0x08 +#define ST_LSM6DSM_TAP_THS_ADDR 0x59 +#define ST_LSM6DSM_TAP_THS_MASK GENMASK(4, 0) +#define ST_LSM6DSM_TAP_DUR_ADDR 0x5a +#define ST_LSM6DSM_TAP_DUR_MASK 0xff +#define ST_LSM6DSM_DTAP_EN_ADDR 0x5b +#define ST_LSM6DSM_DTAP_EN_MASK 0x80 + +/* CUSTOM VALUES FOR WRIST TILT SENSOR */ +#define ST_LSM6DSM_WTILT_EN_ADDR 0x19 +#define ST_LSM6DSM_WTILT_EN_MASK 0x80 +#define ST_LSM6DSM_WTILT_DRDY_IRQ_MASK 0x01 +#define ST_LSM6DSM_WRIST_TILT_IA 0x55 + +#define ST_LSM6DSM_ACCEL_SUFFIX_NAME "accel" +#define ST_LSM6DSM_GYRO_SUFFIX_NAME "gyro" +#define ST_LSM6DSM_STEP_COUNTER_SUFFIX_NAME "step_c" +#define ST_LSM6DSM_STEP_DETECTOR_SUFFIX_NAME "step_d" +#define ST_LSM6DSM_SIGN_MOTION_SUFFIX_NAME "sign_motion" +#define ST_LSM6DSM_TILT_SUFFIX_NAME "tilt" +#define ST_LSM6DSM_WTILT_SUFFIX_NAME "wrist" +#define ST_LSM6DSM_STAP_SUFFIX_NAME "stap" +#define ST_LSM6DSM_DTAP_SUFFIX_NAME "dtap" + +#define ST_LSM6DSM_26HZ_INJECT_NS_UP (ULLONG_MAX) +#define ST_LSM6DSM_26HZ_INJECT_NS_DOWN (25641026LL) +#define ST_LSM6DSM_52HZ_INJECT_NS_UP ST_LSM6DSM_26HZ_INJECT_NS_DOWN +#define ST_LSM6DSM_52HZ_INJECT_NS_DOWN (12820512LL) +#define ST_LSM6DSM_104HZ_INJECT_NS_UP ST_LSM6DSM_52HZ_INJECT_NS_DOWN +#define ST_LSM6DSM_104HZ_INJECT_NS_DOWN (6410256LL) +#define ST_LSM6DSM_208HZ_INJECT_NS_UP ST_LSM6DSM_104HZ_INJECT_NS_DOWN +#define ST_LSM6DSM_208HZ_INJECT_NS_DOWN (0) + +#define ST_LSM6DSM_DEV_ATTR_SAMP_FREQ() \ + IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \ + st_lsm6dsm_sysfs_get_sampling_frequency, \ + st_lsm6dsm_sysfs_set_sampling_frequency) + +#define ST_LSM6DSM_DEV_ATTR_SAMP_FREQ_AVAIL() \ + IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \ + st_lsm6dsm_sysfs_sampling_frequency_avail) + +#define ST_LSM6DSM_DEV_ATTR_SCALE_AVAIL(name) \ + IIO_DEVICE_ATTR(name, S_IRUGO, \ + st_lsm6dsm_sysfs_scale_avail, NULL , 0); + +static struct st_lsm6dsm_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_lsm6dsm_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DSM_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_LSM6DSM_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DSM_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DSM_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DSM_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DSM_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +struct st_lsm6dsm_odr_reg { + unsigned int hz; + u8 value; +}; + +static struct st_lsm6dsm_odr_table { + u8 addr[2]; + u8 mask[2]; + struct st_lsm6dsm_odr_reg odr_avl[ST_LSM6DSM_ODR_LIST_NUM]; +} st_lsm6dsm_odr_table = { + .addr[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_ODR_ADDR, + .mask[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_ODR_MASK, + .addr[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_ODR_ADDR, + .mask[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_ODR_MASK, + .odr_avl[0] = { .hz = 13, .value = ST_LSM6DSM_ODR_13HZ_VAL }, + .odr_avl[1] = { .hz = 26, .value = ST_LSM6DSM_ODR_26HZ_VAL }, + .odr_avl[2] = { .hz = 52, .value = ST_LSM6DSM_ODR_52HZ_VAL }, + .odr_avl[3] = { .hz = 104, .value = ST_LSM6DSM_ODR_104HZ_VAL }, + .odr_avl[4] = { .hz = 208, .value = ST_LSM6DSM_ODR_208HZ_VAL }, + .odr_avl[5] = { .hz = 416, .value = ST_LSM6DSM_ODR_416HZ_VAL }, + .odr_avl[6] = { .hz = 833, .value = ST_LSM6DSM_ODR_833HZ_VAL }, +}; + +struct st_lsm6dsm_fs_reg { + unsigned int gain; + u8 value; +}; + +static struct st_lsm6dsm_fs_table { + u8 addr; + u8 mask; + struct st_lsm6dsm_fs_reg fs_avl[ST_LSM6DSM_FS_LIST_NUM]; +} st_lsm6dsm_fs_table[ST_INDIO_DEV_NUM] = { + [ST_MASK_ID_ACCEL] = { + .addr = ST_LSM6DSM_ACCEL_FS_ADDR, + .mask = ST_LSM6DSM_ACCEL_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DSM_ACCEL_FS_2G_GAIN, + .value = ST_LSM6DSM_ACCEL_FS_2G_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DSM_ACCEL_FS_4G_GAIN, + .value = ST_LSM6DSM_ACCEL_FS_4G_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DSM_ACCEL_FS_8G_GAIN, + .value = ST_LSM6DSM_ACCEL_FS_8G_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DSM_ACCEL_FS_16G_GAIN, + .value = ST_LSM6DSM_ACCEL_FS_16G_VAL }, + }, + [ST_MASK_ID_GYRO] = { + .addr = ST_LSM6DSM_GYRO_FS_ADDR, + .mask = ST_LSM6DSM_GYRO_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DSM_GYRO_FS_250_GAIN, + .value = ST_LSM6DSM_GYRO_FS_250_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DSM_GYRO_FS_500_GAIN, + .value = ST_LSM6DSM_GYRO_FS_500_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DSM_GYRO_FS_1000_GAIN, + .value = ST_LSM6DSM_GYRO_FS_1000_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DSM_GYRO_FS_2000_GAIN, + .value = ST_LSM6DSM_GYRO_FS_2000_VAL }, + } +}; + +static const struct iio_event_spec singol_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, +}; + +const struct iio_event_spec lsm6dsm_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lsm6dsm_accel_ch[] = { + ST_LSM6DSM_LSM_CHANNELS(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DSM_ACCEL_OUT_X_L_ADDR, 's'), + ST_LSM6DSM_LSM_CHANNELS(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DSM_ACCEL_OUT_Y_L_ADDR, 's'), + ST_LSM6DSM_LSM_CHANNELS(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DSM_ACCEL_OUT_Z_L_ADDR, 's'), + ST_LSM6DSM_FLUSH_CHANNEL(IIO_ACCEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6dsm_gyro_ch[] = { + ST_LSM6DSM_LSM_CHANNELS(IIO_ANGL_VEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DSM_GYRO_OUT_X_L_ADDR, 's'), + ST_LSM6DSM_LSM_CHANNELS(IIO_ANGL_VEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DSM_GYRO_OUT_Y_L_ADDR, 's'), + ST_LSM6DSM_LSM_CHANNELS(IIO_ANGL_VEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DSM_GYRO_OUT_Z_L_ADDR, 's'), + ST_LSM6DSM_FLUSH_CHANNEL(IIO_ANGL_VEL), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6dsm_sign_motion_ch[] = { + { + .type = IIO_SIGN_MOTION, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6dsm_step_c_ch[] = { + { + .type = IIO_STEP_COUNTER, + .modified = 0, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .address = ST_LSM6DSM_STEP_COUNTER_OUT_L_ADDR, + .scan_type = { + .sign = 'u', + .realbits = 64, + .storagebits = 64, + .endianness = IIO_LE, + }, + }, + ST_LSM6DSM_FLUSH_CHANNEL(IIO_STEP_COUNTER), + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6dsm_step_d_ch[] = { + ST_LSM6DSM_FLUSH_CHANNEL(IIO_STEP_DETECTOR), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + +static const struct iio_chan_spec st_lsm6dsm_tilt_ch[] = { + ST_LSM6DSM_FLUSH_CHANNEL(IIO_TILT), + IIO_CHAN_SOFT_TIMESTAMP(0) +}; + +static const struct iio_chan_spec st_lsm6dsm_wtilt_ch[] = { + { + .type = IIO_WRIST_TILT_GESTURE, + .modified = 0, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .address = ST_LSM6DSM_WRIST_TILT_IA, + .scan_type = { + .sign = 'u', + .realbits = 8, + .storagebits = 8, + .endianness = IIO_LE, + }, + }, + ST_LSM6DSM_FLUSH_CHANNEL(IIO_WRIST_TILT_GESTURE), + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6dsm_tap_ch[] = { + { + .type = IIO_TAP, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6dsm_tap_tap_ch[] = { + { + .type = IIO_TAP_TAP, + .channel = 0, + .modified = 0, + .event_spec = &singol_thr_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +int st_lsm6dsm_write_data_with_mask(struct lsm6dsm_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = cdata->tf->read(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} +EXPORT_SYMBOL(st_lsm6dsm_write_data_with_mask); + +static inline int st_lsm6dsm_enable_embedded_page_regs(struct lsm6dsm_data *cdata, bool enable) +{ + u8 value = 0x00; + + if (enable) + value = ST_LSM6DSM_FUNC_CFG_REG2_MASK; + + return cdata->tf->write(cdata, ST_LSM6DSM_FUNC_CFG_ACCESS_ADDR, 1, &value, false); +} + +int st_lsm6dsm_write_embedded_registers(struct lsm6dsm_data *cdata, + u8 reg_addr, u8 *data, int len) +{ + int err = 0, err2, count = 0; + + mutex_lock(&cdata->bank_registers_lock); + + if (cdata->enable_digfunc_mask) { + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_FUNC_EN_ADDR, + ST_LSM6DSM_FUNC_EN_MASK, + ST_LSM6DSM_DIS_BIT, false); + if (err < 0) { + mutex_unlock(&cdata->bank_registers_lock); + return err; + } + } + + udelay(100); + + err = st_lsm6dsm_enable_embedded_page_regs(cdata, true); + if (err < 0) + goto restore_digfunc; + + udelay(100); + + err = cdata->tf->write(cdata, reg_addr, len, data, false); + if (err < 0) + goto restore_bank_regs; + + err = st_lsm6dsm_enable_embedded_page_regs(cdata, false); + if (err < 0) + goto restore_bank_regs; + + udelay(100); + + if (cdata->enable_digfunc_mask) { + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_FUNC_EN_ADDR, + ST_LSM6DSM_FUNC_EN_MASK, + ST_LSM6DSM_EN_BIT, false); + if (err < 0) + goto restore_digfunc; + } + + mutex_unlock(&cdata->bank_registers_lock); + + return 0; + +restore_bank_regs: + do { + msleep(200); + err2 = st_lsm6dsm_enable_embedded_page_regs(cdata, false); + } while ((err2 < 0) && (count++ < 10)); + + if (count >= 10) + pr_err("not able to close embedded page registers. It make driver unstable!\n"); + +restore_digfunc: + if (!cdata->enable_digfunc_mask) { + err2 = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_FUNC_EN_ADDR, + ST_LSM6DSM_FUNC_EN_MASK, + ST_LSM6DSM_EN_BIT, false); + } + + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int lsm6dsm_set_watermark(struct lsm6dsm_data *cdata) +{ + int err; + u8 reg_value = 0; + u16 fifo_watermark; + unsigned int fifo_len, sip = 0, min_pattern = UINT_MAX; + + if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_ACCEL] / + cdata->fifo_output[ST_MASK_ID_ACCEL].sip); + } + + if (cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_GYRO].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_GYRO] / + cdata->fifo_output[ST_MASK_ID_GYRO].sip); + } + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if (cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0) { + sip += cdata->fifo_output[ST_MASK_ID_EXT0].sip; + min_pattern = MIN(min_pattern, + cdata->hwfifo_watermark[ST_MASK_ID_EXT0] / + cdata->fifo_output[ST_MASK_ID_EXT0].sip); + } +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + if (sip == 0) + return 0; + + if (min_pattern == 0) + min_pattern = 1; + + min_pattern = MIN(min_pattern, ((unsigned int)ST_LSM6DSM_MAX_FIFO_THRESHOLD / sip)); + + fifo_len = min_pattern * sip * ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE; + fifo_watermark = (fifo_len / 2); + + if (fifo_watermark < (ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE / 2)) + fifo_watermark = ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE / 2; + + if (fifo_watermark != cdata->fifo_watermark) { + err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_THR_H_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + fifo_watermark = (fifo_watermark & ST_LSM6DSM_FIFO_THR_MASK) | + ((reg_value & ~ST_LSM6DSM_FIFO_THR_H_MASK) << 8); + + err = cdata->tf->write(cdata, ST_LSM6DSM_FIFO_THR_L_ADDR, 2, + (u8 *)&fifo_watermark, true); + if (err < 0) + return err; + + cdata->fifo_watermark = fifo_watermark; + } + + return 0; +} + +int st_lsm6dsm_set_fifo_mode(struct lsm6dsm_data *cdata, enum fifo_mode fm) +{ + int err; + u8 reg_value; + + switch (fm) { + case BYPASS: + reg_value = ST_LSM6DSM_FIFO_MODE_BYPASS; + break; + case CONTINUOS: + reg_value = ST_LSM6DSM_FIFO_MODE_CONTINUOS | ST_LSM6DSM_FIFO_ODR_MAX; + break; + default: + return -EINVAL; + } + + err = cdata->tf->write(cdata, ST_LSM6DSM_FIFO_MODE_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (fm != BYPASS) { + cdata->slower_counter = 0; + cdata->fifo_enable_timestamp = + iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = 0; + cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = 0; + } + + cdata->fifo_status = fm; + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsm_set_fifo_mode); + +static int lsm6dsm_write_decimators(struct lsm6dsm_data *cdata, + u8 decimators[3]) +{ + int i; + u8 value[3], decimators_reg[2]; + + for (i = 0; i < 3; i++) { + switch (decimators[i]) { + case 0: + case 1: + case 2: + case 3: + case 4: + value[i] = decimators[i]; + break; + case 8: + value[i] = 0x05; + break; + case 16: + value[i] = 0x06; + break; + case 32: + value[i] = 0x07; + break; + default: + return -EINVAL; + } + } + + decimators_reg[0] = value[0] | (value[1] << 3); + decimators_reg[1] = value[2]; + + return cdata->tf->write(cdata, ST_LSM6DSM_FIFO_DECIMATOR_ADDR, + ARRAY_SIZE(decimators_reg), decimators_reg, true); +} + +static bool lsm6dsm_calculate_fifo_decimators(struct lsm6dsm_data *cdata, + u8 decimators[3], u8 samples_in_pattern[3], + unsigned int new_v_odr[ST_INDIO_DEV_NUM + 1], + unsigned int new_hw_odr[ST_INDIO_DEV_NUM + 1], + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1], + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1]) +{ + unsigned int trigger_odr; + u8 min_decimator, max_decimator = 0; + u8 accel_decimator = 0, gyro_decimator = 0, ext_decimator = 0; + + trigger_odr = new_hw_odr[ST_MASK_ID_ACCEL]; + if (trigger_odr < new_hw_odr[ST_MASK_ID_GYRO]) + trigger_odr = new_hw_odr[ST_MASK_ID_GYRO]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_ACCEL)) && + (new_v_odr[ST_MASK_ID_ACCEL] != 0) && cdata->accel_on) + accel_decimator = trigger_odr / new_v_odr[ST_MASK_ID_ACCEL]; + + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_GYRO)) && + (new_v_odr[ST_MASK_ID_GYRO] != 0) && + (new_hw_odr[ST_MASK_ID_GYRO] > 0)) + gyro_decimator = trigger_odr / new_v_odr[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if ((cdata->sensors_use_fifo & BIT(ST_MASK_ID_EXT0)) && + (new_v_odr[ST_MASK_ID_EXT0] != 0) && cdata->magn_on) + ext_decimator = trigger_odr / new_v_odr[ST_MASK_ID_EXT0]; + + new_fifo_decimator[ST_MASK_ID_EXT0] = 1; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + new_fifo_decimator[ST_MASK_ID_ACCEL] = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = 1; + + if ((accel_decimator != 0) || (gyro_decimator != 0) || (ext_decimator != 0)) { + min_decimator = MIN_BNZ(MIN_BNZ(accel_decimator, gyro_decimator), ext_decimator); + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + if (min_decimator != 1) { + if ((accel_decimator / min_decimator) == 1) { + accel_decimator = 1; + new_fifo_decimator[ST_MASK_ID_ACCEL] = min_decimator; + } else if ((gyro_decimator / min_decimator) == 1) { + gyro_decimator = 1; + new_fifo_decimator[ST_MASK_ID_GYRO] = min_decimator; + } else if ((ext_decimator / min_decimator) == 1) { + ext_decimator = 1; + new_fifo_decimator[ST_MASK_ID_EXT0] = min_decimator; + } + min_decimator = 1; + } + if ((accel_decimator > 4) && (accel_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 3; + accel_decimator = 4; + } else if ((accel_decimator > 8) && (accel_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 7; + accel_decimator = 8; + } else if (accel_decimator > 16 && accel_decimator < 32) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator - 15; + accel_decimator = 16; + } else if (accel_decimator > 32) { + new_fifo_decimator[ST_MASK_ID_ACCEL] = accel_decimator / 32; + accel_decimator = 32; + } + if ((gyro_decimator > 4) && (gyro_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 3; + gyro_decimator = 4; + } else if ((gyro_decimator > 8) && (gyro_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 7; + gyro_decimator = 8; + } else if (gyro_decimator > 16 && gyro_decimator < 32) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator - 15; + gyro_decimator = 16; + } else if (gyro_decimator > 32) { + new_fifo_decimator[ST_MASK_ID_GYRO] = gyro_decimator / 32; + gyro_decimator = 32; + } +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if ((ext_decimator > 4) && (ext_decimator < 8)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 3; + ext_decimator = 4; + } else if ((ext_decimator > 8) && (ext_decimator < 16)) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 7; + ext_decimator = 8; + } else if (ext_decimator > 16 && ext_decimator < 32) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator - 15; + ext_decimator = 16; + } else if (ext_decimator > 32) { + new_fifo_decimator[ST_MASK_ID_EXT0] = ext_decimator / 32; + ext_decimator = 32; + } +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + max_decimator = MAX(MAX(accel_decimator, gyro_decimator), ext_decimator); + } + + decimators[0] = accel_decimator; + if (accel_decimator > 0) { + new_deltatime[ST_MASK_ID_ACCEL] = accel_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[0] = max_decimator / accel_decimator; + } else + samples_in_pattern[0] = 0; + + decimators[1] = gyro_decimator; + if (gyro_decimator > 0) { + new_deltatime[ST_MASK_ID_GYRO] = gyro_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[1] = max_decimator / gyro_decimator; + } else + samples_in_pattern[1] = 0; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + decimators[2] = ext_decimator; + if (ext_decimator > 0) { + new_deltatime[ST_MASK_ID_EXT0] = ext_decimator * + (1000000000U / trigger_odr); + samples_in_pattern[2] = max_decimator / ext_decimator; + } else + samples_in_pattern[2] = 0; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (ext_decimator == cdata->hwfifo_decimator[ST_MASK_ID_EXT0]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { + if (cdata->fifo_output[ST_MASK_ID_EXT0].decimator != new_fifo_decimator[ST_MASK_ID_EXT0]) { + return true; + } +#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + if ((accel_decimator == cdata->hwfifo_decimator[ST_MASK_ID_ACCEL]) && + (gyro_decimator == cdata->hwfifo_decimator[ST_MASK_ID_GYRO])) { +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + return false; + } + + return true; +} + +static int st_lsm6dsm_of_get_drdy_pin(struct lsm6dsm_data *cdata, + int *drdy_pin) +{ + struct device_node *np = cdata->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin); +} + +static int st_lsm6dsm_get_drdy_reg(struct lsm6dsm_data *cdata, u8 *drdy_reg) +{ + int err = 0, drdy_pin; + + if (st_lsm6dsm_of_get_drdy_pin(cdata, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = cdata->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + *drdy_reg = ST_LSM6DSM_INT1_ADDR; + break; + case 2: + *drdy_reg = ST_LSM6DSM_INT2_ADDR; + break; + default: + dev_err(cdata->dev, "unsupported data ready pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +int st_lsm6dsm_set_drdy_irq(struct lsm6dsm_sensor_data *sdata, bool state) +{ + int err; + u16 *irq_mask = NULL; + u8 reg_addr, mask = 0, value; + u16 tmp_irq_enable_fifo_mask, tmp_irq_enable_accel_ext_mask; + + if (state) + value = ST_LSM6DSM_EN_BIT; + else + value = ST_LSM6DSM_DIS_BIT; + + tmp_irq_enable_fifo_mask = + sdata->cdata->irq_enable_fifo_mask & ~sdata->sindex; + tmp_irq_enable_accel_ext_mask = + sdata->cdata->irq_enable_accel_ext_mask & ~sdata->sindex; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = sdata->cdata->drdy_reg; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_ACCEL]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DSM_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_LSM6DSM_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; + case ST_MASK_ID_GYRO: + reg_addr = sdata->cdata->drdy_reg; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_GYRO]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DSM_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else + mask = ST_LSM6DSM_GYRO_DRDY_IRQ_MASK; + + break; + case ST_MASK_ID_SIGN_MOTION: + reg_addr = ST_LSM6DSM_INT1_ADDR; + mask = ST_LSM6DSM_SIGN_MOTION_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_STEP_COUNTER: + reg_addr = ST_LSM6DSM_INT2_ADDR; + mask = ST_LSM6DSM_STEP_COUNTER_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_STEP_DETECTOR: + reg_addr = ST_LSM6DSM_INT1_ADDR; + mask = ST_LSM6DSM_STEP_DETECTOR_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_TILT: + reg_addr = ST_LSM6DSM_MD1_ADDR; + mask = ST_LSM6DSM_TILT_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_WTILT: + reg_addr = ST_LSM6DSM_DRDY_PULSE_CFG_G; + mask = ST_LSM6DSM_WTILT_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_TAP: + reg_addr = ST_LSM6DSM_MD1_ADDR; + mask = ST_LSM6DSM_STAP_DRDY_IRQ_MASK; + break; + case ST_MASK_ID_TAP_TAP: + reg_addr = ST_LSM6DSM_MD1_ADDR; + mask = ST_LSM6DSM_DTAP_DRDY_IRQ_MASK; + break; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + reg_addr = sdata->cdata->drdy_reg; + + if (sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) { + if (tmp_irq_enable_fifo_mask == 0) + mask = ST_LSM6DSM_FIFO_THR_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_fifo_mask; + } else { + if (tmp_irq_enable_accel_ext_mask == 0) + mask = ST_LSM6DSM_ACCEL_DRDY_IRQ_MASK; + + irq_mask = &sdata->cdata->irq_enable_accel_ext_mask; + } + + break; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + default: + return -EINVAL; + } + + if (mask > 0) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); + if (err < 0) + return err; + } + + if (irq_mask != NULL) { + if (state) + *irq_mask |= BIT(sdata->sindex); + else + *irq_mask &= ~BIT(sdata->sindex); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsm_set_drdy_irq); + +static int st_lsm6dsm_set_odr(struct lsm6dsm_sensor_data *sdata, + unsigned int odr, bool force) +{ + u8 reg_value; + int err, i = 0, n; + int64_t temp_last_timestamp[3] = { 0 }; + bool scan_odr = true, fifo_conf_changed; + unsigned int temp_v_odr[ST_INDIO_DEV_NUM + 1]; + unsigned int temp_hw_odr[ST_INDIO_DEV_NUM + 1]; + int64_t new_deltatime[ST_INDIO_DEV_NUM + 1] = { 0 }; + short new_fifo_decimator[ST_INDIO_DEV_NUM + 1] = { 0 }; + u8 fifo_decimator[3] = { 0 }, samples_in_pattern[3] = { 0 }; + u8 temp_num_samples[3] = { 0 }, temp_old_decimator[3] = { 1 }; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + + if (scan_odr) { + for (i = 0; i < ST_LSM6DSM_ODR_LIST_NUM; i++) { + if (st_lsm6dsm_odr_table.odr_avl[i].hz == odr) + break; + } + if (i == ST_LSM6DSM_ODR_LIST_NUM) + return -EINVAL; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = st_lsm6dsm_odr_table.odr_avl[i].hz; + return 0; + } + } + + if (sdata->cdata->hw_odr[sdata->sindex] == st_lsm6dsm_odr_table.odr_avl[i].hz) + reg_value = 0xff; + else + reg_value = st_lsm6dsm_odr_table.odr_avl[i].value; + } else + reg_value = ST_LSM6DSM_ODR_POWER_OFF_VAL; + + if (sdata->cdata->sensors_use_fifo > 0) { + /* someone is using fifo */ + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + temp_v_odr[ST_MASK_ID_GYRO] = sdata->cdata->v_odr[ST_MASK_ID_GYRO]; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (force) + temp_v_odr[ST_MASK_ID_ACCEL] = sdata->cdata->accel_odr_dependency[0]; + + temp_hw_odr[ST_MASK_ID_ACCEL] = odr; + temp_hw_odr[ST_MASK_ID_GYRO] = sdata->cdata->hw_odr[ST_MASK_ID_GYRO]; + } else { + if (!force) + temp_v_odr[ST_MASK_ID_GYRO] = odr; + + temp_hw_odr[ST_MASK_ID_GYRO] = odr; + temp_hw_odr[ST_MASK_ID_ACCEL] = sdata->cdata->hw_odr[ST_MASK_ID_ACCEL]; + } +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + temp_v_odr[ST_MASK_ID_EXT0] = sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + fifo_conf_changed = lsm6dsm_calculate_fifo_decimators(sdata->cdata, + fifo_decimator, samples_in_pattern, temp_v_odr, + temp_hw_odr, new_deltatime, new_fifo_decimator); + if (fifo_conf_changed) { + /* FIFO configuration changed, needs to write new decimators */ + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_lsm6dsm_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + err = st_lsm6dsm_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } + + err = lsm6dsm_write_decimators(sdata->cdata, fifo_decimator); + if (err < 0) + goto reenable_fifo_irq; + + if (reg_value != 0xff) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + st_lsm6dsm_odr_table.addr[sdata->sindex], + st_lsm6dsm_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_208HZ; + break; + } + } + + sdata->cdata->hwfifo_decimator[ST_MASK_ID_ACCEL] = fifo_decimator[0]; + sdata->cdata->hwfifo_decimator[ST_MASK_ID_GYRO] = fifo_decimator[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator = new_fifo_decimator[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator = new_fifo_decimator[ST_MASK_ID_GYRO]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = new_fifo_decimator[ST_MASK_ID_ACCEL] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = new_fifo_decimator[ST_MASK_ID_GYRO] - 1; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip = samples_in_pattern[0]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip = samples_in_pattern[1]; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default = new_deltatime[ST_MASK_ID_ACCEL]; + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + sdata->cdata->hwfifo_decimator[ST_MASK_ID_EXT0] = fifo_decimator[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator = new_fifo_decimator[ST_MASK_ID_EXT0]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = new_fifo_decimator[ST_MASK_ID_EXT0] - 1; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip = samples_in_pattern[2]; + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + err = lsm6dsm_set_watermark(sdata->cdata); + if (err < 0) + goto reenable_fifo_irq; + + if ((samples_in_pattern[0] > 0) || (samples_in_pattern[1] > 0) || (samples_in_pattern[2] > 0)) { + err = st_lsm6dsm_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_lsm6dsm_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_lsm6dsm_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_lsm6dsm_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } else { + /* FIFO configuration not changed */ + + if (reg_value == 0xff) { + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + return 0; + } + + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) { + st_lsm6dsm_read_fifo(sdata->cdata, true); + + temp_num_samples[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip; + temp_num_samples[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip; + temp_last_timestamp[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p; + temp_last_timestamp[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p; + temp_old_decimator[0] = sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].decimator; + temp_old_decimator[1] = sdata->cdata->fifo_output[ST_MASK_ID_GYRO].decimator; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + temp_num_samples[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip; + temp_last_timestamp[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p; + temp_old_decimator[2] = sdata->cdata->fifo_output[ST_MASK_ID_EXT0].decimator; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + err = st_lsm6dsm_set_fifo_mode(sdata->cdata, BYPASS); + if (err < 0) + goto reenable_fifo_irq; + } else { + temp_num_samples[0] = 0; + temp_num_samples[1] = 0; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + temp_num_samples[2] = 0; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + st_lsm6dsm_odr_table.addr[sdata->sindex], + st_lsm6dsm_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) + goto reenable_fifo_irq; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (temp_hw_odr[ST_MASK_ID_ACCEL]) { + case 13: + case 26: + case 52: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_52HZ; + break; + case 104: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_104HZ; + break; + default: + if (temp_num_samples[0] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_208HZ; + break; + } + } + + switch (temp_hw_odr[ST_MASK_ID_GYRO]) { + case 13: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_13HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_13HZ; + break; + case 26: + case 52: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_52HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_52HZ; + break; + case 104: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_104HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_104HZ; + break; + default: + if (temp_num_samples[1] == 0) + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_208HZ; + else + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_208HZ; + break; + } + + if ((sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].sip > 0) || + (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].sip > 0)) { + err = st_lsm6dsm_set_fifo_mode(sdata->cdata, CONTINUOS); + if (err < 0) + goto reenable_fifo_irq; + + if (((temp_num_samples[0] > 0) && (samples_in_pattern[0] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[0]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime * temp_old_decimator[0]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[0], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[0] += temp_deltatime; + err = st_lsm6dsm_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_ACCEL, + sdata->cdata->accel_last_push, temp_last_timestamp[0]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp_p = temp_last_timestamp[0]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = new_deltatime[ST_MASK_ID_ACCEL]; + + if (((temp_num_samples[1] > 0) && (samples_in_pattern[1] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_GYRO].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[1]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime * temp_old_decimator[1]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[1], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[1] += temp_deltatime; + err = st_lsm6dsm_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_GYRO, + sdata->cdata->gyro_last_push, temp_last_timestamp[1]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].timestamp_p = temp_last_timestamp[1]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = new_deltatime[ST_MASK_ID_GYRO]; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if (((temp_num_samples[2] > 0) && (samples_in_pattern[2] > 0)) && (sdata->cdata->fifo_output[ST_MASK_ID_EXT0].initialized)) { + unsigned int n_gen; + int64_t temp_deltatime = 0; + + if (sdata->cdata->fifo_enable_timestamp > temp_last_timestamp[2]) { + n_gen = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime * temp_old_decimator[2]); + + if (n_gen > 0) + temp_deltatime = div64_s64(sdata->cdata->fifo_enable_timestamp - temp_last_timestamp[2], n_gen); + + for (n = 0; n < n_gen; n++) { + temp_last_timestamp[2] += temp_deltatime; + err = st_lsm6dsm_push_data_with_timestamp(sdata->cdata, ST_MASK_ID_EXT0, + sdata->cdata->ext0_last_push, temp_last_timestamp[2]); + if (err < 0) + break; + + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = temp_last_timestamp[2]; + } + } + } else + sdata->cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = new_deltatime[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + if (temp_v_odr[sdata->sindex] != 0) + sdata->cdata->v_odr[sdata->sindex] = temp_v_odr[sdata->sindex]; + + sdata->cdata->hw_odr[sdata->sindex] = temp_hw_odr[sdata->sindex]; + } else { + /* no one is using FIFO */ + + disable_irq(sdata->cdata->irq); + + if ((odr != 0) && (sdata->cdata->hw_odr[sdata->sindex] == st_lsm6dsm_odr_table.odr_avl[i].hz)) { + if (sdata->sindex == ST_MASK_ID_ACCEL) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + + return 0; + } + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + st_lsm6dsm_odr_table.addr[sdata->sindex], + st_lsm6dsm_odr_table.mask[sdata->sindex], + reg_value, true); + if (err < 0) { + enable_irq(sdata->cdata->irq); + return err; + } + + if (!force) + sdata->cdata->v_odr[sdata->sindex] = st_lsm6dsm_odr_table.odr_avl[i].hz; + + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = st_lsm6dsm_odr_table.odr_avl[i].hz; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + switch (sdata->cdata->hw_odr[sdata->sindex]) { + case 13: + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_ACCEL] = ST_LSM6DSM_ACCEL_STD_208HZ; + break; + } + } + + switch (sdata->cdata->hw_odr[ST_MASK_ID_GYRO]) { + case 13: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_13HZ; + break; + case 26: + case 52: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_52HZ; + break; + case 104: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_104HZ; + break; + default: + sdata->cdata->samples_to_discard[ST_MASK_ID_GYRO] = ST_LSM6DSM_GYRO_STD_208HZ; + break; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (sdata->cdata->hw_odr[sdata->sindex] > 0) { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_ACCEL]; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = + sdata->cdata->hw_odr[ST_MASK_ID_ACCEL] / sdata->cdata->v_odr[ST_MASK_ID_EXT0]; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } else { + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator = 1; + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator = 1; + } + + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator - 1; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = + sdata->cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator - 1; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + } + + enable_irq(sdata->cdata->irq); + } + + sdata->cdata->trigger_odr = sdata->cdata->hw_odr[0] > sdata->cdata->hw_odr[1] ? sdata->cdata->hw_odr[0] : sdata->cdata->hw_odr[1]; + + return 0; + +reenable_fifo_irq: + enable_irq(sdata->cdata->irq); + return err; +} + +/* + * Enable / disable accelerometer + */ +static int lsm6dsm_enable_accel(struct lsm6dsm_data *cdata, enum st_mask_id id, int min_odr) +{ + int odr; + struct lsm6dsm_sensor_data *sdata_accel = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + switch (id) { + case ST_MASK_ID_ACCEL: + cdata->accel_odr_dependency[0] = min_odr; + if (min_odr > 0) + cdata->accel_on = true; + else + cdata->accel_on = false; + + break; + case ST_MASK_ID_SENSOR_HUB: + cdata->accel_odr_dependency[1] = min_odr; + if (min_odr > 0) + cdata->magn_on = true; + else + cdata->magn_on = false; + + break; + case ST_MASK_ID_DIGITAL_FUNC: + cdata->accel_odr_dependency[2] = min_odr; + break; + default: + return -EINVAL; + } + + if (cdata->accel_odr_dependency[0] > cdata->accel_odr_dependency[1]) + odr = cdata->accel_odr_dependency[0]; + else + odr = cdata->accel_odr_dependency[1]; + + if (cdata->accel_odr_dependency[2] > odr) + odr = cdata->accel_odr_dependency[2]; + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + if (cdata->injection_mode) + return 0; +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + return st_lsm6dsm_set_odr(sdata_accel, odr, true); +} + +/* + * Enable / disable digital func + */ +static int lsm6dsm_enable_digital_func(struct lsm6dsm_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_digfunc_mask == 0) { + err = lsm6dsm_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 26); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_FUNC_EN_ADDR, + ST_LSM6DSM_FUNC_EN_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask |= BIT(id); + } else { + if ((cdata->enable_digfunc_mask & ~BIT(id)) == 0) { + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_FUNC_EN_ADDR, + ST_LSM6DSM_FUNC_EN_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 0); + if (err < 0) + return err; + } + cdata->enable_digfunc_mask &= ~BIT(id); + + } + + return 0; +} + +/* + * Enable / disable HW pedometer + */ +static int lsm6dsm_enable_pedometer(struct lsm6dsm_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_pedometer_mask == 0) { + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_PEDOMETER_EN_ADDR, + ST_LSM6DSM_PEDOMETER_EN_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_digital_func(cdata, + true, ST_MASK_ID_HW_PEDOMETER); + if (err < 0) + return err; + } + cdata->enable_pedometer_mask |= BIT(id); + } else { + if ((cdata->enable_pedometer_mask & ~BIT(id)) == 0) { + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_PEDOMETER_EN_ADDR, + ST_LSM6DSM_PEDOMETER_EN_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_digital_func(cdata, + false, ST_MASK_ID_HW_PEDOMETER); + if (err < 0) + return err; + } + cdata->enable_pedometer_mask &= ~BIT(id); + } + + return 0; +} + +static int lsm6dsm_enable_tap(struct lsm6dsm_sensor_data *sdata, + bool enable) +{ + int err; + struct lsm6dsm_data *cdata = sdata->cdata; + bool stap_en = (cdata->sensors_enabled & BIT(ST_MASK_ID_TAP)); + bool dtap_en = (cdata->sensors_enabled & BIT(ST_MASK_ID_TAP_TAP)); + + if (enable) { + if (!stap_en && !dtap_en) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_LIR_ADDR, + ST_LSM6DSM_INT_ENABLE_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_LIR_ADDR, + ST_LSM6DSM_LIR_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_TAP_EN_ADDR, ST_LSM6DSM_TAP_EN_MASK, + 0x7, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 416); + if (err < 0) + return err; + } + } else { + if (((sdata->sindex == ST_MASK_ID_TAP) && dtap_en) || + ((sdata->sindex == ST_MASK_ID_TAP_TAP) && stap_en)) { + /* do not disable tap */ + return 0; + } + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_LIR_ADDR, + ST_LSM6DSM_INT_ENABLE_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_LIR_ADDR, + ST_LSM6DSM_LIR_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_accel(cdata, + ST_MASK_ID_DIGITAL_FUNC, 0); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_TAP_EN_ADDR, ST_LSM6DSM_TAP_EN_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + } + return 0; +} + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT +int st_lsm6dsm_enable_sensor_hub(struct lsm6dsm_data *cdata, + bool enable, enum st_mask_id id) +{ + int err; + + if (enable) { + if (cdata->enable_sensorhub_mask == 0) { + err = lsm6dsm_enable_digital_func(cdata, + true, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + + err = lsm6dsm_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_SENSORHUB_ADDR, + ST_LSM6DSM_SENSORHUB_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + } else + err = lsm6dsm_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask |= BIT(id); + } else { + if ((cdata->enable_sensorhub_mask & ~BIT(id)) == 0) { + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_SENSORHUB_ADDR, + ST_LSM6DSM_SENSORHUB_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_accel(cdata, + ST_MASK_ID_SENSOR_HUB, 0); + if (err < 0) + return err; + + err = lsm6dsm_enable_digital_func(cdata, + false, ST_MASK_ID_SENSOR_HUB); + if (err < 0) + return err; + } else + err = lsm6dsm_enable_accel(cdata, ST_MASK_ID_SENSOR_HUB, + cdata->v_odr[ST_MASK_ID_EXT0]); + + cdata->enable_sensorhub_mask &= ~BIT(id); + } + + return err < 0 ? err : 0; +} +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + +int st_lsm6dsm_set_enable(struct lsm6dsm_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + err = lsm6dsm_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, + enable ? sdata->cdata->v_odr[ST_MASK_ID_ACCEL] : 0); + if (err < 0) + return 0; + + break; + case ST_MASK_ID_GYRO: + err = st_lsm6dsm_set_odr(sdata, enable ? + sdata->cdata->v_odr[ST_MASK_ID_GYRO] : 0, true); + if (err < 0) + return err; + + break; + case ST_MASK_ID_SIGN_MOTION: + if (enable) + reg_value = ST_LSM6DSM_EN_BIT; + else + reg_value = ST_LSM6DSM_DIS_BIT; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_SIGN_MOTION_EN_ADDR, + ST_LSM6DSM_SIGN_MOTION_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_SIGN_MOTION); + if (err < 0) + return err; + + break; + case ST_MASK_ID_STEP_COUNTER: + if (enable) + reg_value = ST_LSM6DSM_EN_BIT; + else + reg_value = ST_LSM6DSM_DIS_BIT; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_TIMER_EN_ADDR, + ST_LSM6DSM_TIMER_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_STEP_COUNTER); + if (err < 0) + return err; + + break; + case ST_MASK_ID_STEP_DETECTOR: + err = lsm6dsm_enable_pedometer(sdata->cdata, + enable, ST_MASK_ID_STEP_DETECTOR); + if (err < 0) + return err; + + break; + case ST_MASK_ID_TILT: + if (enable) + reg_value = ST_LSM6DSM_EN_BIT; + else + reg_value = ST_LSM6DSM_DIS_BIT; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_TILT_EN_ADDR, + ST_LSM6DSM_TILT_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_digital_func(sdata->cdata, + enable, ST_MASK_ID_TILT); + if (err < 0) + return err; + + break; + case ST_MASK_ID_WTILT: + if (enable) + reg_value = ST_LSM6DSM_EN_BIT; + else + reg_value = ST_LSM6DSM_DIS_BIT; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_WTILT_EN_ADDR, + ST_LSM6DSM_WTILT_EN_MASK, + reg_value, true); + if (err < 0) + return err; + + err = lsm6dsm_enable_digital_func(sdata->cdata, + enable, ST_MASK_ID_WTILT); + if (err < 0) + return err; + + break; + case ST_MASK_ID_TAP: + case ST_MASK_ID_TAP_TAP: + err = lsm6dsm_enable_tap(sdata, enable); + if (err < 0) + return err; + + break; + default: + return -EINVAL; + } + + if (buffer) { + err = st_lsm6dsm_set_drdy_irq(sdata, enable); + if (err < 0) + return err; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; +} + +static int st_lsm6dsm_set_fs(struct lsm6dsm_sensor_data *sdata, + unsigned int gain) +{ + int err, i; + u8 pedometer_reg_value; + + for (i = 0; i < ST_LSM6DSM_FS_LIST_NUM; i++) { + if (st_lsm6dsm_fs_table[sdata->sindex].fs_avl[i].gain == gain) + break; + } + if (i == ST_LSM6DSM_FS_LIST_NUM) + return -EINVAL; + + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + st_lsm6dsm_fs_table[sdata->sindex].addr, + st_lsm6dsm_fs_table[sdata->sindex].mask, + st_lsm6dsm_fs_table[sdata->sindex].fs_avl[i].value, + true); + if (err < 0) + return err; + + sdata->c_gain[0] = gain; + + if (sdata->sindex == ST_MASK_ID_ACCEL) { + if (i == 0) + pedometer_reg_value = ST_LSM6DSM_STEP_COUNTER_THS_2G_VALUE; + else + pedometer_reg_value = ST_LSM6DSM_STEP_COUNTER_THS_4G_VALUE; + + st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_STEP_COUNTER_THS_ADDR, + &pedometer_reg_value, 1); + } + + return 0; +} + +static int st_lsm6dsm_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[ST_LSM6DSM_BYTE_FOR_CHANNEL]; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6dsm_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (sdata->sindex == ST_MASK_ID_ACCEL) + msleep(40); + + if (sdata->sindex == ST_MASK_ID_GYRO) + msleep(120); + + err = sdata->cdata->tf->read(sdata->cdata, ch->address, + ST_LSM6DSM_BYTE_FOR_CHANNEL, outdata, true); + if (err < 0) { + st_lsm6dsm_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + st_lsm6dsm_set_enable(sdata, false, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[0]; + return IIO_VAL_INT_PLUS_NANO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6dsm_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long mask) +{ + int err; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = st_lsm6dsm_set_fs(sdata, val2); + mutex_unlock(&indio_dev->mlock); + break; + default: + return -EINVAL; + } + + return err < 0 ? err : 0; +} + +static int st_lsm6dsm_reset_steps(struct lsm6dsm_data *cdata) +{ + int err; + u8 reg_value = 0x00; + + err = cdata->tf->read(cdata, + ST_LSM6DSM_STEP_COUNTER_RES_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (reg_value & ST_LSM6DSM_FUNC_EN_MASK) + reg_value = ST_LSM6DSM_STEP_COUNTER_RES_FUNC_EN; + else + reg_value = ST_LSM6DSM_DIS_BIT; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_STEP_COUNTER_RES_ADDR, + ST_LSM6DSM_STEP_COUNTER_RES_MASK, + ST_LSM6DSM_STEP_COUNTER_RES_ALL_EN, true); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_STEP_COUNTER_RES_ADDR, + ST_LSM6DSM_STEP_COUNTER_RES_MASK, + reg_value, true); + if (err < 0) + return err; + + cdata->reset_steps = true; + + return 0; +} + +static int st_lsm6dsm_init_sensor(struct lsm6dsm_data *cdata) +{ + int err; + u8 default_reg_value = ST_LSM6DSM_RESET_MASK; + + err = cdata->tf->write(cdata, ST_LSM6DSM_RESET_ADDR, 1, + &default_reg_value, true); + if (err < 0) + return err; + + msleep(200); + + /* Latch interrupts */ + err = st_lsm6dsm_write_data_with_mask(cdata, ST_LSM6DSM_LIR_ADDR, + ST_LSM6DSM_LIR_MASK, ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + /* Enable BDU for sensors data */ + err = st_lsm6dsm_write_data_with_mask(cdata, ST_LSM6DSM_BDU_ADDR, + ST_LSM6DSM_BDU_MASK, ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_ROUNDING_ADDR, + ST_LSM6DSM_ROUNDING_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + /* Set tap and tap-tap threshold */ + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_TAP_THS_ADDR, + ST_LSM6DSM_TAP_THS_MASK, + 0xc, true); + if (err < 0) + return err; + + /* configure int duration */ + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_TAP_DUR_ADDR, + ST_LSM6DSM_TAP_DUR_MASK, + 0x7f, true); + if (err < 0) + return err; + + /* configure double tap */ + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_DTAP_EN_ADDR, + ST_LSM6DSM_DTAP_EN_MASK, + 1, true); + if (err < 0) + return err; + + /* Redirect INT2 on INT1, all interrupt will be available on INT1 */ + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_INT2_ON_INT1_ADDR, + ST_LSM6DSM_INT2_ON_INT1_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6dsm_reset_steps(cdata); + if (err < 0) + return err; + + default_reg_value = 0x00; + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_STEP_COUNTER_DURATION_ADDR, + &default_reg_value, 1); + if (err < 0) + return err; + + default_reg_value = ST_LSM6DSM_STEP_COUNTER_THS_2G_VALUE; + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_STEP_COUNTER_THS_ADDR, + &default_reg_value, 1); + if (err < 0) + return err; + + return st_lsm6dsm_get_drdy_reg(cdata, &cdata->drdy_reg); +} + +static int st_lsm6dsm_set_selftest(struct lsm6dsm_sensor_data *sdata, int index) +{ + u8 mode, mask; + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + mask = ST_LSM6DSM_SELFTEST_ACCEL_MASK; + mode = st_lsm6dsm_selftest_table[index].accel_value; + break; + case ST_MASK_ID_GYRO: + mask = ST_LSM6DSM_SELFTEST_GYRO_MASK; + mode = st_lsm6dsm_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_SELFTEST_ADDR, mask, mode, true); +} + +static ssize_t st_lsm6dsm_sysfs_set_max_delivery_rate(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u8 duration; + int err; + unsigned int max_delivery_rate; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtouint(buf, 10, &max_delivery_rate); + if (err < 0) + return -EINVAL; + + if (max_delivery_rate == sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER]) + return size; + + duration = max_delivery_rate / ST_LSM6DSM_MIN_DURATION_MS; + + err = st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_STEP_COUNTER_DURATION_ADDR, + &duration, 1); + if (err < 0) + return err; + + sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER] = max_delivery_rate; + + return size; +} + +static ssize_t st_lsm6dsm_sysfs_get_max_delivery_rate(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6dsm_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", + sdata->cdata->v_odr[ST_MASK_ID_STEP_COUNTER]); +} + +static ssize_t st_lsm6dsm_sysfs_reset_counter(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + struct lsm6dsm_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + err = st_lsm6dsm_reset_steps(sdata->cdata); + if (err < 0) + return err; + + return size; +} + +static ssize_t st_lsm6dsm_sysfs_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6dsm_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_lsm6dsm_sysfs_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + if (odr == 12) + odr++; + + mutex_lock(&indio_dev->mlock); + + mutex_lock(&sdata->cdata->odr_lock); +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + if (!((sdata->sindex & ST_MASK_ID_ACCEL) && + sdata->cdata->injection_mode)) { + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_lsm6dsm_set_odr(sdata, odr, false); + } +#else /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + if (sdata->cdata->v_odr[sdata->sindex] != odr) { + if ((sdata->sindex == ST_MASK_ID_ACCEL) && (sdata->cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL))) + err = lsm6dsm_enable_accel(sdata->cdata, ST_MASK_ID_ACCEL, odr); + else + err = st_lsm6dsm_set_odr(sdata, odr, false); + } +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + mutex_unlock(&sdata->cdata->odr_lock); + + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6dsm_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < ST_LSM6DSM_ODR_LIST_NUM; i++) { + if (st_lsm6dsm_odr_table.odr_avl[i].hz == 13) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", 12); + else + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lsm6dsm_odr_table.odr_avl[i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsm_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + struct lsm6dsm_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + for (i = 0; i < ST_LSM6DSM_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_lsm6dsm_fs_table[sdata->sindex].fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsm_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6dsm_selftest_table[1].string_mode, + st_lsm6dsm_selftest_table[2].string_mode); +} + +static ssize_t st_lsm6dsm_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + result = sdata->cdata->accel_selftest_status; + break; + case ST_MASK_ID_GYRO: + result = sdata->cdata->gyro_selftest_status; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_LSM6DSM_SELFTEST_NA_MS; + else if (result < 0) + message = ST_LSM6DSM_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_LSM6DSM_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_lsm6dsm_sysfs_start_selftest_status(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i, n; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + u8 reg_status, reg_addr, temp_reg_status, outdata[6]; + int x = 0, y = 0, z = 0, x_selftest = 0, y_selftest = 0, z_selftest = 0; + + mutex_lock(&sdata->cdata->odr_lock); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + sdata->cdata->accel_selftest_status = 0; + break; + case ST_MASK_ID_GYRO: + sdata->cdata->gyro_selftest_status = 0; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + for (n = 0; n < ARRAY_SIZE(st_lsm6dsm_selftest_table); n++) { + if (strncmp(buf, st_lsm6dsm_selftest_table[n].string_mode, + size - 2) == 0) + break; + } + if (n == ARRAY_SIZE(st_lsm6dsm_selftest_table)) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + reg_addr = ST_LSM6DSM_SELFTEST_ACCEL_ADDR; + temp_reg_status = ST_LSM6DSM_SELFTEST_ACCEL_REG_VALUE; + break; + case ST_MASK_ID_GYRO: + reg_addr = ST_LSM6DSM_SELFTEST_GYRO_ADDR; + temp_reg_status = ST_LSM6DSM_SELFTEST_GYRO_REG_VALUE; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = sdata->cdata->tf->read(sdata->cdata, + reg_addr, 1, ®_status, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, &temp_reg_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 20; + y += ((s16)*(u16 *)&outdata[2]) / 20; + z += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = st_lsm6dsm_set_selftest(sdata, n); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + /* get data with selftest enabled */ + msleep(100); + + for (i = 0; i < 20; i++) { + err = sdata->cdata->tf->read(sdata->cdata, + sdata->data_out_reg, 6, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 20; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 20; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 20; + + mdelay(10); + } + + err = sdata->cdata->tf->write(sdata->cdata, + reg_addr, 1, ®_status, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + err = st_lsm6dsm_set_selftest(sdata, 0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + if ((abs(x_selftest - x) < ST_LSM6DSM_SELFTEST_ACCEL_MIN) || + (abs(x_selftest - x) > ST_LSM6DSM_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_LSM6DSM_SELFTEST_ACCEL_MIN) || + (abs(y_selftest - y) > ST_LSM6DSM_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_LSM6DSM_SELFTEST_ACCEL_MIN) || + (abs(z_selftest - z) > ST_LSM6DSM_SELFTEST_ACCEL_MAX)) { + sdata->cdata->accel_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->accel_selftest_status = 1; + break; + case ST_MASK_ID_GYRO: + if ((abs(x_selftest - x) < ST_LSM6DSM_SELFTEST_GYRO_MIN) || + (abs(x_selftest - x) > ST_LSM6DSM_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(y_selftest - y) < ST_LSM6DSM_SELFTEST_GYRO_MIN) || + (abs(y_selftest - y) > ST_LSM6DSM_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + if ((abs(z_selftest - z) < ST_LSM6DSM_SELFTEST_GYRO_MIN) || + (abs(z_selftest - z) > ST_LSM6DSM_SELFTEST_GYRO_MAX)) { + sdata->cdata->gyro_selftest_status = -1; + goto selftest_failure; + } + + sdata->cdata->gyro_selftest_status = 1; + break; + default: + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + +selftest_failure: + mutex_unlock(&sdata->cdata->odr_lock); + + return size; +} + +ssize_t st_lsm6dsm_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u64 sensor_last_timestamp, event_type = 0; + int stype = 0; + u64 timestamp_flush = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_lock(&sdata->cdata->odr_lock); + disable_irq(sdata->cdata->irq); + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + sensor_last_timestamp = + sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + st_lsm6dsm_read_fifo(sdata->cdata, true); + + if (sensor_last_timestamp == + sdata->cdata->fifo_output[sdata->sindex].timestamp_p) + event_type = IIO_EV_DIR_FIFO_EMPTY; + else + event_type = IIO_EV_DIR_FIFO_DATA; + + timestamp_flush = sdata->cdata->fifo_output[sdata->sindex].timestamp_p; + + enable_irq(sdata->cdata->irq); + + switch (sdata->sindex) { + case ST_MASK_ID_ACCEL: + stype = IIO_ACCEL; + break; + + case ST_MASK_ID_GYRO: + stype = IIO_ANGL_VEL; + break; + +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + case ST_MASK_ID_EXT0: + stype = IIO_MAGN; + break; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + } + + iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(stype, + -1, IIO_EV_TYPE_FIFO_FLUSH, event_type), + timestamp_flush); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +ssize_t st_lsm6dsm_sysfs_get_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_enabled[sdata->sindex]); +} + +ssize_t st_lsm6dsm_sysfs_set_hwfifo_enabled(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + bool enable = false; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + err = -EBUSY; + goto set_hwfifo_enabled_unlock_mutex; + } + + err = strtobool(buf, &enable); + if (err < 0) + goto set_hwfifo_enabled_unlock_mutex; + + mutex_lock(&sdata->cdata->odr_lock); + + sdata->cdata->hwfifo_enabled[sdata->sindex] = enable; + + if (enable) + sdata->cdata->sensors_use_fifo |= BIT(sdata->sindex); + else + sdata->cdata->sensors_use_fifo &= ~BIT(sdata->sindex); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; + +set_hwfifo_enabled_unlock_mutex: + mutex_unlock(&indio_dev->mlock); + return err; +} + +ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", + sdata->cdata->hwfifo_watermark[sdata->sindex]); +} + +ssize_t st_lsm6dsm_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err = 0, watermark = 0, old_watermark; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if ((watermark < 1) || (watermark > ST_LSM6DSM_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) && + (sdata->cdata->sensors_use_fifo & BIT(sdata->sindex))) { + disable_irq(sdata->cdata->irq); + + if (sdata->cdata->fifo_status != BYPASS) + st_lsm6dsm_read_fifo(sdata->cdata, true); + + old_watermark = sdata->cdata->hwfifo_watermark[sdata->sindex]; + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + err = lsm6dsm_set_watermark(sdata->cdata); + if (err < 0) + sdata->cdata->hwfifo_watermark[sdata->sindex] = old_watermark; + + enable_irq(sdata->cdata->irq); + } else + sdata->cdata->hwfifo_watermark[sdata->sindex] = watermark; + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : size; +} + +ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", ST_LSM6DSM_MAX_FIFO_LENGHT); +} + +ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION +static ssize_t st_lsm6dsm_sysfs_set_injection_mode(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, start; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = kstrtoint(buf, 10, &start); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + mutex_lock(&sdata->cdata->odr_lock); + + if (start == 0) { + /* End injection */ + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_TEST_REG_ADDR, + ST_LSM6DSM_START_INJECT_XL_MASK, 0, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + /* Force accel ODR to 26Hz if dependencies are enabled */ + if (sdata->cdata->sensors_enabled > 0) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + st_lsm6dsm_odr_table.addr[sdata->sindex], + st_lsm6dsm_odr_table.mask[sdata->sindex], + st_lsm6dsm_odr_table.odr_avl[1].value, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + } + + sdata->cdata->injection_mode = false; + } else { + sdata->cdata->last_injection_timestamp = 0; + sdata->cdata->injection_odr = 0; + + /* Set start injection */ + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_TEST_REG_ADDR, + ST_LSM6DSM_START_INJECT_XL_MASK, 1, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + sdata->cdata->injection_mode = true; + } + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return size; +} + +static ssize_t st_lsm6dsm_sysfs_get_injection_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", sdata->cdata->injection_mode); +} + +static ssize_t st_lsm6dsm_sysfs_upload_xl_data(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i, n = 1; + s64 timestamp, deltatime; + u8 sample[3], current_odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (!sdata->cdata->injection_mode) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + for (i = 0; i < 3; i++) + sample[i] = *(s16 *)(&buf[i * 2]) >> 8; + + timestamp = *(s64 *)(buf + ALIGN(6, sizeof(s64))); + + if (sdata->cdata->last_injection_timestamp > 0) { + deltatime = timestamp - sdata->cdata->last_injection_timestamp; + if ((deltatime > ST_LSM6DSM_208HZ_INJECT_NS_DOWN) && + (deltatime <= ST_LSM6DSM_208HZ_INJECT_NS_UP)) { + current_odr = 208; + n = 4; + } else if ((deltatime > ST_LSM6DSM_104HZ_INJECT_NS_DOWN) && + (deltatime <= ST_LSM6DSM_104HZ_INJECT_NS_UP)) { + current_odr = 104; + n = 3; + } else if ((deltatime > ST_LSM6DSM_52HZ_INJECT_NS_DOWN) && + (deltatime <= ST_LSM6DSM_52HZ_INJECT_NS_UP)) { + current_odr = 52; + n = 2; + } else if ((deltatime > ST_LSM6DSM_26HZ_INJECT_NS_DOWN) && + (deltatime <= ST_LSM6DSM_26HZ_INJECT_NS_UP)) { + current_odr = 26; + n = 1; + } else { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + if (sdata->cdata->injection_odr != current_odr) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + st_lsm6dsm_odr_table.addr[sdata->sindex], + st_lsm6dsm_odr_table.mask[sdata->sindex], + st_lsm6dsm_odr_table.odr_avl[n].value, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + sdata->cdata->injection_odr = current_odr; + } + } + + sdata->cdata->last_injection_timestamp = timestamp; + + err = sdata->cdata->tf->write(sdata->cdata, ST_LSM6DSM_INJECT_XL_X_ADDR, + 3, (u8 *)sample, false); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + mutex_unlock(&indio_dev->mlock); + + usleep_range(1000, 2000); + + return size; +} + +static ssize_t st_lsm6dsm_sysfs_get_injection_sensors(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", "lsm6dsm_accel"); +} +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + +ssize_t st_lsm6dsm_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + struct lsm6dsm_data *cdata = sdata->cdata; + + return scnprintf(buf, PAGE_SIZE, "%u\n", cdata->module_id); +} + +static ST_LSM6DSM_DEV_ATTR_SAMP_FREQ(); +static ST_LSM6DSM_DEV_ATTR_SAMP_FREQ_AVAIL(); +static ST_LSM6DSM_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); +static ST_LSM6DSM_DEV_ATTR_SCALE_AVAIL(in_anglvel_scale_available); + +static ST_LSM6DSM_HWFIFO_ENABLED(); +static ST_LSM6DSM_HWFIFO_WATERMARK(); +static ST_LSM6DSM_HWFIFO_WATERMARK_MIN(); +static ST_LSM6DSM_HWFIFO_WATERMARK_MAX(); +static ST_LSM6DSM_HWFIFO_FLUSH(); + +static IIO_DEVICE_ATTR(reset_counter, S_IWUSR, + NULL, st_lsm6dsm_sysfs_reset_counter, 0); + +static IIO_DEVICE_ATTR(max_delivery_rate, S_IWUSR | S_IRUGO, + st_lsm6dsm_sysfs_get_max_delivery_rate, + st_lsm6dsm_sysfs_set_max_delivery_rate, 0); + +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_lsm6dsm_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_lsm6dsm_sysfs_get_selftest_status, + st_lsm6dsm_sysfs_start_selftest_status, 0); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsm_get_module_id, NULL, 0); + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION +static IIO_DEVICE_ATTR(injection_mode, S_IWUSR | S_IRUGO, + st_lsm6dsm_sysfs_get_injection_mode, + st_lsm6dsm_sysfs_set_injection_mode, 0); + +static IIO_DEVICE_ATTR(in_accel_injection_raw, S_IWUSR, NULL, + st_lsm6dsm_sysfs_upload_xl_data, 0); + +static IIO_DEVICE_ATTR(injection_sensors, S_IRUGO, + st_lsm6dsm_sysfs_get_injection_sensors, + NULL, 0); +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + +static int st_lsm6dsm_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == IIO_CHAN_INFO_SCALE) { + if ((chan->type == IIO_ANGL_VEL) || + (chan->type == IIO_ACCEL)) + return IIO_VAL_INT_PLUS_NANO; + } + + return -EINVAL; +} + +static struct attribute *st_lsm6dsm_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_mode.dev_attr.attr, + &iio_dev_attr_in_accel_injection_raw.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_accel_attribute_group = { + .attrs = st_lsm6dsm_accel_attributes, +}; + +static const struct iio_info st_lsm6dsm_accel_info = { + .attrs = &st_lsm6dsm_accel_attribute_group, + .read_raw = &st_lsm6dsm_read_raw, + .write_raw = &st_lsm6dsm_write_raw, + .write_raw_get_fmt = st_lsm6dsm_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dsm_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.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_lsm6dsm_gyro_attribute_group = { + .attrs = st_lsm6dsm_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsm_gyro_info = { + .attrs = &st_lsm6dsm_gyro_attribute_group, + .read_raw = &st_lsm6dsm_read_raw, + .write_raw = &st_lsm6dsm_write_raw, + .write_raw_get_fmt = st_lsm6dsm_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dsm_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_sign_motion_attribute_group = { + .attrs = st_lsm6dsm_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6dsm_sign_motion_info = { + .attrs = &st_lsm6dsm_sign_motion_attribute_group, +}; + +static struct attribute *st_lsm6dsm_step_c_attributes[] = { + &iio_dev_attr_reset_counter.dev_attr.attr, + &iio_dev_attr_max_delivery_rate.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_step_c_attribute_group = { + .attrs = st_lsm6dsm_step_c_attributes, +}; + +static const struct iio_info st_lsm6dsm_step_c_info = { + .attrs = &st_lsm6dsm_step_c_attribute_group, + .read_raw = &st_lsm6dsm_read_raw, +}; + +static struct attribute *st_lsm6dsm_step_d_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_step_d_attribute_group = { + .attrs = st_lsm6dsm_step_d_attributes, +}; + +static const struct iio_info st_lsm6dsm_step_d_info = { + .attrs = &st_lsm6dsm_step_d_attribute_group, +}; + +static struct attribute *st_lsm6dsm_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_tilt_attribute_group = { + .attrs = st_lsm6dsm_tilt_attributes, +}; + +static const struct iio_info st_lsm6dsm_tilt_info = { + .attrs = &st_lsm6dsm_tilt_attribute_group, +}; + +static struct attribute *st_lsm6dsm_wtilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_wtilt_attribute_group = { + .attrs = st_lsm6dsm_wtilt_attributes, +}; + +static const struct iio_info st_lsm6dsm_wtilt_info = { + .attrs = &st_lsm6dsm_wtilt_attribute_group, +}; + +static struct attribute *st_lsm6dsm_tap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_tap_attribute_group = { + .attrs = st_lsm6dsm_tap_attributes, +}; + +static const struct iio_info st_lsm6dsm_tap_info = { + .attrs = &st_lsm6dsm_tap_attribute_group, +}; + +static struct attribute *st_lsm6dsm_tap_tap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + &iio_dev_attr_injection_sensors.dev_attr.attr, +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_tap_tap_attribute_group = { + .attrs = st_lsm6dsm_tap_tap_attributes, +}; + +static const struct iio_info st_lsm6dsm_tap_tap_info = { + .attrs = &st_lsm6dsm_tap_tap_attribute_group, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops st_lsm6dsm_trigger_ops = { + .set_trigger_state = ST_LSM6DSM_TRIGGER_SET_STATE, +}; +#define ST_LSM6DSM_TRIGGER_OPS (&st_lsm6dsm_trigger_ops) +#else +#define ST_LSM6DSM_TRIGGER_OPS NULL +#endif + +static void st_lsm6dsm_get_properties(struct lsm6dsm_data *cdata) +{ + if (device_property_read_u32(cdata->dev, "st,module_id", + &cdata->module_id)) { + cdata->module_id = 1; + } +} + +int st_lsm6dsm_common_probe(struct lsm6dsm_data *cdata, int irq) +{ + u8 wai = 0x00; + int i, n, err; + struct lsm6dsm_sensor_data *sdata; + + mutex_init(&cdata->bank_registers_lock); + mutex_init(&cdata->fifo_lock); + mutex_init(&cdata->tb.buf_lock); + mutex_init(&cdata->odr_lock); + + cdata->fifo_watermark = 0; + cdata->fifo_status = BYPASS; + cdata->enable_digfunc_mask = 0; + cdata->enable_pedometer_mask = 0; +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + cdata->enable_sensorhub_mask = 0; +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + cdata->irq_enable_fifo_mask = 0; + cdata->irq_enable_accel_ext_mask = 0; + + for (i = 0; i < ST_INDIO_DEV_NUM + 1; i++) { + cdata->hw_odr[i] = 0; + cdata->v_odr[i] = 0; + cdata->hwfifo_enabled[i] = false; + cdata->hwfifo_decimator[i] = 0; + cdata->hwfifo_watermark[i] = 1; + cdata->nofifo_decimation[i].decimator = 1; + cdata->nofifo_decimation[i].num_samples = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].decimator = 1; + cdata->fifo_output[i].timestamp_p = 0; + cdata->fifo_output[i].sip = 0; + cdata->fifo_output[i].initialized = false; + } + + cdata->sensors_use_fifo = 0; + cdata->sensors_enabled = 0; + + cdata->gyro_selftest_status = 0; + cdata->accel_selftest_status = 0; + + cdata->accel_on = false; + cdata->magn_on = false; + + cdata->reset_steps = false; + cdata->num_steps = 0; + + cdata->accel_odr_dependency[0] = 0; + cdata->accel_odr_dependency[1] = 0; + cdata->accel_odr_dependency[2] = 0; + + cdata->trigger_odr = 0; + + cdata->fifo_data = kmalloc(ST_LSM6DSM_MAX_FIFO_SIZE * + sizeof(u8), GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + cdata->injection_mode = false; + cdata->last_injection_timestamp = 0; + cdata->injection_odr = 0; +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + err = cdata->tf->read(cdata, ST_LSM6DSM_WAI_ADDRESS, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + goto free_fifo_data; + } + if (wai != ST_LSM6DSM_WAI_EXP) { + dev_err(cdata->dev, + "Who-Am-I value not valid. Expected %x, Found %x\n", + ST_LSM6DSM_WAI_EXP, wai); + err = -ENODEV; + goto free_fifo_data; + } + + st_lsm6dsm_get_properties(cdata); + + if (irq > 0) { + cdata->irq = irq; + } else { + err = -EINVAL; + dev_info(cdata->dev, + "DRDY not available, curernt implementation needs irq!\n"); + goto free_fifo_data; + } + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + cdata->indio_dev[i] = devm_iio_device_alloc(cdata->dev, + sizeof(struct lsm6dsm_sensor_data)); + if (!cdata->indio_dev[i]) { + err = -ENOMEM; + goto free_fifo_data; + } + + sdata = iio_priv(cdata->indio_dev[i]); + sdata->cdata = cdata; + sdata->sindex = i; + + switch (i) { + case ST_MASK_ID_ACCEL: + sdata->data_out_reg = st_lsm6dsm_accel_ch[0].address; + cdata->v_odr[i] = st_lsm6dsm_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_lsm6dsm_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_GYRO: + sdata->data_out_reg = st_lsm6dsm_gyro_ch[0].address; + cdata->v_odr[i] = st_lsm6dsm_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = st_lsm6dsm_fs_table[i].fs_avl[0].gain; + sdata->cdata->samples_to_discard_2[ST_MASK_ID_GYRO] = 0; + sdata->num_data_channels = 3; + break; + case ST_MASK_ID_STEP_COUNTER: + sdata->data_out_reg = st_lsm6dsm_step_c_ch[0].address; + sdata->num_data_channels = 1; + break; + case ST_MASK_ID_WTILT: + sdata->data_out_reg = st_lsm6dsm_wtilt_ch[0].address; + sdata->num_data_channels = 1; + break; + + default: + sdata->num_data_channels = 0; + break; + } + + cdata->indio_dev[i]->modes = INDIO_DIRECT_MODE; + } + + cdata->indio_dev[ST_MASK_ID_ACCEL]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_ACCEL_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_ACCEL]->info = &st_lsm6dsm_accel_info; + cdata->indio_dev[ST_MASK_ID_ACCEL]->channels = st_lsm6dsm_accel_ch; + cdata->indio_dev[ST_MASK_ID_ACCEL]->num_channels = + ARRAY_SIZE(st_lsm6dsm_accel_ch); + + cdata->indio_dev[ST_MASK_ID_GYRO]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_GYRO_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_GYRO]->info = &st_lsm6dsm_gyro_info; + cdata->indio_dev[ST_MASK_ID_GYRO]->channels = st_lsm6dsm_gyro_ch; + cdata->indio_dev[ST_MASK_ID_GYRO]->num_channels = + ARRAY_SIZE(st_lsm6dsm_gyro_ch); + + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_SIGN_MOTION_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->info = + &st_lsm6dsm_sign_motion_info; + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->channels = + st_lsm6dsm_sign_motion_ch; + cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]->num_channels = + ARRAY_SIZE(st_lsm6dsm_sign_motion_ch); + + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_STEP_COUNTER_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->info = + &st_lsm6dsm_step_c_info; + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->channels = + st_lsm6dsm_step_c_ch; + cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]->num_channels = + ARRAY_SIZE(st_lsm6dsm_step_c_ch); + + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_STEP_DETECTOR_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->info = + &st_lsm6dsm_step_d_info; + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->channels = + st_lsm6dsm_step_d_ch; + cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]->num_channels = + ARRAY_SIZE(st_lsm6dsm_step_d_ch); + + cdata->indio_dev[ST_MASK_ID_TILT]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_TILT_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_TILT]->info = &st_lsm6dsm_tilt_info; + cdata->indio_dev[ST_MASK_ID_TILT]->channels = st_lsm6dsm_tilt_ch; + cdata->indio_dev[ST_MASK_ID_TILT]->num_channels = + ARRAY_SIZE(st_lsm6dsm_tilt_ch); + + cdata->indio_dev[ST_MASK_ID_WTILT]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_WTILT_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_WTILT]->info = &st_lsm6dsm_wtilt_info; + cdata->indio_dev[ST_MASK_ID_WTILT]->channels = st_lsm6dsm_wtilt_ch; + cdata->indio_dev[ST_MASK_ID_WTILT]->num_channels = + ARRAY_SIZE(st_lsm6dsm_wtilt_ch); + + cdata->indio_dev[ST_MASK_ID_TAP]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_STAP_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_TAP]->info = &st_lsm6dsm_tap_info; + cdata->indio_dev[ST_MASK_ID_TAP]->channels = st_lsm6dsm_tap_ch; + cdata->indio_dev[ST_MASK_ID_TAP]->num_channels = + ARRAY_SIZE(st_lsm6dsm_tap_ch); + + cdata->indio_dev[ST_MASK_ID_TAP_TAP]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DSM_DTAP_SUFFIX_NAME); + cdata->indio_dev[ST_MASK_ID_TAP_TAP]->info = &st_lsm6dsm_tap_tap_info; + cdata->indio_dev[ST_MASK_ID_TAP_TAP]->channels = st_lsm6dsm_tap_tap_ch; + cdata->indio_dev[ST_MASK_ID_TAP_TAP]->num_channels = + ARRAY_SIZE(st_lsm6dsm_tap_tap_ch); + + err = st_lsm6dsm_init_sensor(cdata); + if (err < 0) + goto free_fifo_data; + + err = st_lsm6dsm_allocate_rings(cdata); + if (err < 0) + goto free_fifo_data; + + if (irq > 0) { + err = st_lsm6dsm_allocate_triggers(cdata, + ST_LSM6DSM_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_device_register(cdata->indio_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + st_lsm6dsm_i2c_master_probe(cdata); + + device_init_wakeup(cdata->dev, true); + + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->indio_dev[n]); + + if (irq > 0) + st_lsm6dsm_deallocate_triggers(cdata); +deallocate_ring: + st_lsm6dsm_deallocate_rings(cdata); +free_fifo_data: + kfree(cdata->fifo_data); + + return err; +} +EXPORT_SYMBOL(st_lsm6dsm_common_probe); + +void st_lsm6dsm_common_remove(struct lsm6dsm_data *cdata, int irq) +{ + int i; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_device_unregister(cdata->indio_dev[i]); + + if (irq > 0) + st_lsm6dsm_deallocate_triggers(cdata); + + st_lsm6dsm_deallocate_rings(cdata); + + kfree(cdata->fifo_data); + + st_lsm6dsm_i2c_master_exit(cdata); +} +EXPORT_SYMBOL(st_lsm6dsm_common_remove); + +#ifdef CONFIG_PM +int __maybe_unused st_lsm6dsm_common_suspend(struct lsm6dsm_data *cdata) +{ + int err, i; + u8 tmp_sensors_enabled; + struct lsm6dsm_sensor_data *sdata; + + tmp_sensors_enabled = cdata->sensors_enabled; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_MASK_ID_SIGN_MOTION) || (i == ST_MASK_ID_TILT) || + (i == ST_MASK_ID_WTILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + +#ifdef CONFIG_ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND + if ((BIT(i) & cdata->sensors_enabled) && + (i == ST_MASK_ID_STEP_COUNTER)) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_INT2_ADDR, + ST_LSM6DSM_STEP_COUNTER_DRDY_IRQ_MASK, + ST_LSM6DSM_DIS_BIT, true); + if (err < 0) + return err; + + continue; + } +#endif /* CONFIG_ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND */ + + err = st_lsm6dsm_set_enable(sdata, false, true); + if (err < 0) + return err; + } + cdata->sensors_enabled = tmp_sensors_enabled; + + if (cdata->sensors_enabled & ST_LSM6DSM_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + enable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsm_common_suspend); + +int __maybe_unused st_lsm6dsm_common_resume(struct lsm6dsm_data *cdata) +{ + int err, i; + struct lsm6dsm_sensor_data *sdata; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_MASK_ID_SIGN_MOTION) || (i == ST_MASK_ID_TILT) || + (i == ST_MASK_ID_WTILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + + if (BIT(sdata->sindex) & cdata->sensors_enabled) { +#ifdef CONFIG_ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND + if (i == ST_MASK_ID_STEP_COUNTER) { + err = st_lsm6dsm_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_INT2_ADDR, + ST_LSM6DSM_STEP_COUNTER_DRDY_IRQ_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; + + continue; + } +#endif /* CONFIG_ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND */ + + err = st_lsm6dsm_set_enable(sdata, true, true); + if (err < 0) + return err; + } + } + + if (cdata->sensors_enabled & ST_LSM6DSM_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + disable_irq_wake(cdata->irq); + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsm_common_resume); +#endif /* CONFIG_PM */ + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6dsm core driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c.c b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c.c new file mode 100644 index 000000000000..b0c3c8868e45 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c.c @@ -0,0 +1,180 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6dsm i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsm.h" + +static int st_lsm6dsm_i2c_read(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int st_lsm6dsm_i2c_write(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + int err = 0; + u8 send[8]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, &msg, 1); + + return err; +} + +static const struct st_lsm6dsm_transfer_function st_lsm6dsm_tf_i2c = { + .write = st_lsm6dsm_i2c_write, + .read = st_lsm6dsm_i2c_read, +}; + +static int st_lsm6dsm_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct lsm6dsm_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + i2c_set_clientdata(client, cdata); + + cdata->tf = &st_lsm6dsm_tf_i2c; + + err = st_lsm6dsm_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6dsm_i2c_remove(struct i2c_client *client) +{ + struct lsm6dsm_data *cdata = i2c_get_clientdata(client); + + st_lsm6dsm_common_remove(cdata, client->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_lsm6dsm_suspend(struct device *dev) +{ + struct lsm6dsm_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6dsm_common_suspend(cdata); +} + +static int __maybe_unused st_lsm6dsm_resume(struct device *dev) +{ + struct lsm6dsm_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6dsm_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6dsm_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsm_suspend, st_lsm6dsm_resume) +}; + +#define ST_LSM6DSM_PM_OPS (&st_lsm6dsm_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DSM_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id st_lsm6dsm_id_table[] = { + { LSM6DSM_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsm_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id lsm6dsm_of_match[] = { + { + .compatible = "st,lsm6dsm", + .data = LSM6DSM_DEV_NAME, + }, + { + .compatible = "st,lsm6dsl", + .data = LSM6DSL_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, lsm6dsm_of_match); +#else /* CONFIG_OF */ +#define lsm6dsm_of_match NULL +#endif /* CONFIG_OF */ + +static struct i2c_driver st_lsm6dsm_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6dsm-i2c", + .pm = ST_LSM6DSM_PM_OPS, + .of_match_table = of_match_ptr(lsm6dsm_of_match), + }, + .probe = st_lsm6dsm_i2c_probe, + .remove = st_lsm6dsm_i2c_remove, + .id_table = st_lsm6dsm_id_table, +}; +module_i2c_driver(st_lsm6dsm_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6dsm i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c_master.c b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c_master.c new file mode 100644 index 000000000000..2b5988a0541d --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c_master.c @@ -0,0 +1,1776 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6dsm i2c master driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0) +#include +#endif /* LINUX_VERSION_CODE */ + +#include "st_lsm6dsm.h" + +#define EXT0_INDEX 0 + +#define ST_LSM6DSM_ODR_LIST_NUM 4 +#define ST_LSM6DSM_SENSOR_HUB_OP_TIMEOUT 5 +#define ST_LSM6DSM_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DSM_EN_BIT 0x01 +#define ST_LSM6DSM_DIS_BIT 0x00 +#define ST_LSM6DSM_SLV0_ADDR_ADDR 0x02 +#define ST_LSM6DSM_SLV1_ADDR_ADDR 0x05 +#define ST_LSM6DSM_SLV2_ADDR_ADDR 0x08 +#define ST_LSM6DSM_SLV0_OUT_ADDR 0x2e +#define ST_LSM6DSM_INTER_PULLUP_ADDR 0x1a +#define ST_LSM6DSM_INTER_PULLUP_MASK 0x08 +#define ST_LSM6DSM_FUNC_MAX_RATE_ADDR 0x18 +#define ST_LSM6DSM_FUNC_MAX_RATE_MASK 0x02 +#define ST_LSM6DSM_DATAWRITE_SLV0 0x0e +#define ST_LSM6DSM_SLVX_READ 0x01 + +/* External sensors configuration */ +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6dsm_sensor_data *sdata); + +#define ST_LSM6DSM_EXT0_ADDR 0x1e +#define ST_LSM6DSM_EXT0_ADDR2 0x1c +#define ST_LSM6DSM_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DSM_EXT0_WAI_VALUE 0x3d +#define ST_LSM6DSM_EXT0_RESET_ADDR 0x21 +#define ST_LSM6DSM_EXT0_RESET_MASK 0x04 +#define ST_LSM6DSM_EXT0_FULLSCALE_ADDR 0x21 +#define ST_LSM6DSM_EXT0_FULLSCALE_MASK 0x60 +#define ST_LSM6DSM_EXT0_FULLSCALE_VALUE 0x02 +#define ST_LSM6DSM_EXT0_ODR_ADDR 0x20 +#define ST_LSM6DSM_EXT0_ODR_MASK 0x1c +#define ST_LSM6DSM_EXT0_ODR0_HZ 10 +#define ST_LSM6DSM_EXT0_ODR0_VALUE 0x04 +#define ST_LSM6DSM_EXT0_ODR1_HZ 20 +#define ST_LSM6DSM_EXT0_ODR1_VALUE 0x05 +#define ST_LSM6DSM_EXT0_ODR2_HZ 40 +#define ST_LSM6DSM_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DSM_EXT0_ODR3_HZ 80 +#define ST_LSM6DSM_EXT0_ODR3_VALUE 0x07 +#define ST_LSM6DSM_EXT0_PW_ADDR 0x22 +#define ST_LSM6DSM_EXT0_PW_MASK 0x03 +#define ST_LSM6DSM_EXT0_PW_OFF 0x02 +#define ST_LSM6DSM_EXT0_PW_ON 0x00 +#define ST_LSM6DSM_EXT0_GAIN_VALUE 438 +#define ST_LSM6DSM_EXT0_OUT_X_L_ADDR 0x28 +#define ST_LSM6DSM_EXT0_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DSM_EXT0_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DSM_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DSM_EXT0_BDU_ADDR 0x24 +#define ST_LSM6DSM_EXT0_BDU_MASK 0x40 +#define ST_LSM6DSM_EXT0_STD 0 +#define ST_LSM6DSM_EXT0_BOOT_FUNCTION (&lis3mdl_initialization) +#define ST_LSM6DSM_SELFTEST_EXT0_MIN 2281 +#define ST_LSM6DSM_SELFTEST_EXT0_MAX 6843 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN_Z 228 +#define ST_LSM6DSM_SELFTEST_EXT0_MAX_Z 2281 +#define ST_LSM6DSM_SELFTEST_ADDR1 0x20 +#define ST_LSM6DSM_SELFTEST_ADDR2 0x21 +#define ST_LSM6DSM_SELFTEST_ADDR3 0x22 +#define ST_LSM6DSM_SELFTEST_ADDR1_VALUE 0x1c +#define ST_LSM6DSM_SELFTEST_ADDR2_VALUE 0x40 +#define ST_LSM6DSM_SELFTEST_ADDR3_VALUE 0x00 +#define ST_LSM6DSM_SELFTEST_ENABLE 0x1d +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct lsm6dsm_sensor_data *sdata); + +#define ST_LSM6DSM_EXT0_ADDR 0x0c +#define ST_LSM6DSM_EXT0_ADDR2 0x0d +#define ST_LSM6DSM_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DSM_EXT0_WAI_VALUE 0x05 +#define ST_LSM6DSM_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DSM_EXT0_RESET_MASK 0x01 +#define ST_LSM6DSM_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DSM_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DSM_EXT0_ODR_MASK 0x1f +#define ST_LSM6DSM_EXT0_ODR0_HZ 10 +#define ST_LSM6DSM_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DSM_EXT0_ODR1_HZ 20 +#define ST_LSM6DSM_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DSM_EXT0_ODR2_HZ 50 +#define ST_LSM6DSM_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DSM_EXT0_ODR3_HZ 100 +#define ST_LSM6DSM_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DSM_EXT0_PW_ADDR ST_LSM6DSM_EXT0_ODR_ADDR +#define ST_LSM6DSM_EXT0_PW_MASK ST_LSM6DSM_EXT0_ODR_MASK +#define ST_LSM6DSM_EXT0_PW_OFF 0x00 +#define ST_LSM6DSM_EXT0_PW_ON ST_LSM6DSM_EXT0_ODR0_VALUE +#define ST_LSM6DSM_EXT0_GAIN_VALUE 6000 +#define ST_LSM6DSM_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DSM_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DSM_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DSM_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DSM_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DSM_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DSM_EXT0_STD 0 +#define ST_LSM6DSM_EXT0_BOOT_FUNCTION (&akm09911_initialization) +#define ST_LSM6DSM_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN (-30) +#define ST_LSM6DSM_SELFTEST_EXT0_MAX 30 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN_Z (-400) +#define ST_LSM6DSM_SELFTEST_EXT0_MAX_Z (-50) +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6dsm_sensor_data *sdata); + +#define ST_LSM6DSM_EXT0_ADDR 0x0c +#define ST_LSM6DSM_EXT0_ADDR2 0x0d +#define ST_LSM6DSM_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DSM_EXT0_WAI_VALUE 0x04 +#define ST_LSM6DSM_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DSM_EXT0_RESET_MASK 0x01 +#define ST_LSM6DSM_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DSM_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DSM_EXT0_ODR_MASK 0x1f +#define ST_LSM6DSM_EXT0_ODR0_HZ 10 +#define ST_LSM6DSM_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DSM_EXT0_ODR1_HZ 20 +#define ST_LSM6DSM_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DSM_EXT0_ODR2_HZ 50 +#define ST_LSM6DSM_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DSM_EXT0_ODR3_HZ 100 +#define ST_LSM6DSM_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DSM_EXT0_PW_ADDR ST_LSM6DSM_EXT0_ODR_ADDR +#define ST_LSM6DSM_EXT0_PW_MASK ST_LSM6DSM_EXT0_ODR_MASK +#define ST_LSM6DSM_EXT0_PW_OFF 0x00 +#define ST_LSM6DSM_EXT0_PW_ON ST_LSM6DSM_EXT0_ODR0_VALUE +#define ST_LSM6DSM_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DSM_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DSM_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DSM_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DSM_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DSM_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DSM_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DSM_EXT0_STD 0 +#define ST_LSM6DSM_EXT0_BOOT_FUNCTION (&akm09912_initialization) +#define ST_LSM6DSM_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN (-200) +#define ST_LSM6DSM_SELFTEST_EXT0_MAX 200 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN_Z (-1600) +#define ST_LSM6DSM_SELFTEST_EXT0_MAX_Z (-400) +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09916 +#define ST_LSM6DSM_EXT0_ADDR 0x0c +#define ST_LSM6DSM_EXT0_ADDR2 0x0c +#define ST_LSM6DSM_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DSM_EXT0_WAI_VALUE 0x09 +#define ST_LSM6DSM_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DSM_EXT0_RESET_MASK 0x01 +#define ST_LSM6DSM_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DSM_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DSM_EXT0_ODR_MASK 0x1f +#define ST_LSM6DSM_EXT0_ODR0_HZ 10 +#define ST_LSM6DSM_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DSM_EXT0_ODR1_HZ 20 +#define ST_LSM6DSM_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DSM_EXT0_ODR2_HZ 50 +#define ST_LSM6DSM_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DSM_EXT0_ODR3_HZ 100 +#define ST_LSM6DSM_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DSM_EXT0_PW_ADDR ST_LSM6DSM_EXT0_ODR_ADDR +#define ST_LSM6DSM_EXT0_PW_MASK ST_LSM6DSM_EXT0_ODR_MASK +#define ST_LSM6DSM_EXT0_PW_OFF 0x00 +#define ST_LSM6DSM_EXT0_PW_ON ST_LSM6DSM_EXT0_ODR0_VALUE +#define ST_LSM6DSM_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DSM_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DSM_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DSM_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DSM_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DSM_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DSM_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DSM_EXT0_STD 0 +#define ST_LSM6DSM_EXT0_BOOT_FUNCTION NULL +#define ST_LSM6DSM_EXT0_DATA_STATUS 0x18 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN (-200) +#define ST_LSM6DSM_SELFTEST_EXT0_MAX 200 +#define ST_LSM6DSM_SELFTEST_EXT0_MIN_Z (-1000) +#define ST_LSM6DSM_SELFTEST_EXT0_MAX_Z (-200) +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09916 */ + + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct lsm6dsm_sensor_data *sdata); + +#define ST_LSM6DSM_EXT0_ADDR 0x5d +#define ST_LSM6DSM_EXT0_ADDR2 0x5c +#define ST_LSM6DSM_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DSM_EXT0_WAI_VALUE 0xb1 +#define ST_LSM6DSM_EXT0_RESET_ADDR 0x11 +#define ST_LSM6DSM_EXT0_RESET_MASK 0x80 +#define ST_LSM6DSM_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DSM_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DSM_EXT0_ODR_ADDR 0x10 +#define ST_LSM6DSM_EXT0_ODR_MASK 0x70 +#define ST_LSM6DSM_EXT0_ODR0_HZ 1 +#define ST_LSM6DSM_EXT0_ODR0_VALUE 0x01 +#define ST_LSM6DSM_EXT0_ODR1_HZ 10 +#define ST_LSM6DSM_EXT0_ODR1_VALUE 0x02 +#define ST_LSM6DSM_EXT0_ODR2_HZ 25 +#define ST_LSM6DSM_EXT0_ODR2_VALUE 0x03 +#define ST_LSM6DSM_EXT0_ODR3_HZ 50 +#define ST_LSM6DSM_EXT0_ODR3_VALUE 0x04 +#define ST_LSM6DSM_EXT0_PW_ADDR ST_LSM6DSM_EXT0_ODR_ADDR +#define ST_LSM6DSM_EXT0_PW_MASK ST_LSM6DSM_EXT0_ODR_MASK +#define ST_LSM6DSM_EXT0_PW_OFF 0x00 +#define ST_LSM6DSM_EXT0_PW_ON ST_LSM6DSM_EXT0_ODR0_VALUE +#define ST_LSM6DSM_EXT0_GAIN_VALUE 244 +#define ST_LSM6DSM_EXT0_OUT_P_L_ADDR 0x28 +#define ST_LSM6DSM_EXT0_OUT_T_L_ADDR 0x2b +#define ST_LSM6DSM_EXT0_READ_DATA_LEN 5 +#define ST_LSM6DSM_EXT0_BDU_ADDR 0x10 +#define ST_LSM6DSM_EXT0_BDU_MASK 0x02 +#define ST_LSM6DSM_EXT0_STD 0 +#define ST_LSM6DSM_EXT0_BOOT_FUNCTION (&lps22hb_initialization) +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS2MDL +static int lis2mdl_initialization(struct lsm6dsm_sensor_data *sdata); + +#define ST_LSM6DSM_EXT0_ADDR 0x1e +#define ST_LSM6DSM_EXT0_ADDR2 0x1e +#define ST_LSM6DSM_EXT0_WAI_ADDR 0x4f +#define ST_LSM6DSM_EXT0_WAI_VALUE 0x40 +#define ST_LSM6DSM_EXT0_RESET_ADDR 0x60 +#define ST_LSM6DSM_EXT0_RESET_MASK 0x20 +#define ST_LSM6DSM_EXT0_ODR_ADDR 0x60 +#define ST_LSM6DSM_EXT0_ODR_MASK 0x0c +#define ST_LSM6DSM_EXT0_ODR0_HZ 10 +#define ST_LSM6DSM_EXT0_ODR0_VALUE 0x00 +#define ST_LSM6DSM_EXT0_ODR1_HZ 20 +#define ST_LSM6DSM_EXT0_ODR1_VALUE 0x01 +#define ST_LSM6DSM_EXT0_ODR2_HZ 50 +#define ST_LSM6DSM_EXT0_ODR2_VALUE 0x02 +#define ST_LSM6DSM_EXT0_ODR3_HZ 100 +#define ST_LSM6DSM_EXT0_ODR3_VALUE 0x03 +#define ST_LSM6DSM_EXT0_PW_ADDR 0x60 +#define ST_LSM6DSM_EXT0_PW_MASK 0x03 +#define ST_LSM6DSM_EXT0_PW_OFF 0x02 +#define ST_LSM6DSM_EXT0_PW_ON 0x00 +#define ST_LSM6DSM_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DSM_EXT0_OUT_X_L_ADDR 0x68 +#define ST_LSM6DSM_EXT0_OUT_Y_L_ADDR 0x6a +#define ST_LSM6DSM_EXT0_OUT_Z_L_ADDR 0x6c +#define ST_LSM6DSM_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DSM_EXT0_BDU_ADDR 0x62 +#define ST_LSM6DSM_EXT0_BDU_MASK 0x10 +#define ST_LSM6DSM_EXT0_STD 0 +#define ST_LSM6DSM_EXT0_TEMP_COMP_ADDR 0x60 +#define ST_LSM6DSM_EXT0_TEMP_COMP_MASK 0x80 +#define ST_LSM6DSM_EXT0_OFF_CANC_ADDR 0x61 +#define ST_LSM6DSM_EXT0_OFF_CANC_MASK 0x02 +#define ST_LSM6DSM_EXT0_BOOT_FUNCTION (&lis2mdl_initialization) +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS2MDL */ + +/* SENSORS SUFFIX NAMES */ +#define ST_LSM6DSM_EXT0_SUFFIX_NAME "magn" +#define ST_LSM6DSM_EXT1_SUFFIX_NAME "press" + +#if defined(CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911) +#define ST_LSM6DSM_EXT0_HAS_SELFTEST 1 +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_MAGN */ + +#if defined(CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB) +#define ST_LSM6DSM_EXT0_HAS_FULLSCALE 1 +#endif + +#if defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09916) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911) +#define ST_LSM6DSM_EXT0_IS_AKM 1 +#define ST_LSM6DSM_SELFTEST_STATUS_REG 0x10 +#define ST_LSM6DSM_SELFTEST_ADDR 0x31 +#define ST_LSM6DSM_SELFTEST_ENABLE 0x10 +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM0099xx */ + + +struct st_lsm6dsm_i2c_master_odr_reg { + unsigned int hz; + u8 value; +}; + +struct st_lsm6dsm_i2c_master_odr_table { + u8 addr; + u8 mask; + struct st_lsm6dsm_i2c_master_odr_reg odr_avl[ST_LSM6DSM_ODR_LIST_NUM]; +}; + +static int st_lsm6dsm_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask); + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB +static const struct iio_chan_spec st_lsm6dsm_ext0_ch[] = { + ST_LSM6DSM_LSM_CHANNELS(IIO_PRESSURE, 0, 0, IIO_NO_MOD, IIO_LE, + 24, 24, ST_LSM6DSM_EXT0_OUT_P_L_ADDR, 'u'), + ST_LSM6DSM_LSM_CHANNELS(IIO_TEMP, 0, 1, IIO_NO_MOD, IIO_LE, + 16, 16, ST_LSM6DSM_EXT0_OUT_T_L_ADDR, 's'), + ST_LSM6DSM_FLUSH_CHANNEL(IIO_PRESSURE), + IIO_CHAN_SOFT_TIMESTAMP(2) +}; +#else /* CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB */ +static const struct iio_chan_spec st_lsm6dsm_ext0_ch[] = { + ST_LSM6DSM_LSM_CHANNELS(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DSM_EXT0_OUT_X_L_ADDR, 's'), + ST_LSM6DSM_LSM_CHANNELS(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DSM_EXT0_OUT_Y_L_ADDR, 's'), + ST_LSM6DSM_LSM_CHANNELS(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DSM_EXT0_OUT_Z_L_ADDR, 's'), + ST_LSM6DSM_FLUSH_CHANNEL(IIO_MAGN), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB */ + +static int st_lsm6dsm_i2c_master_set_odr(struct lsm6dsm_sensor_data *sdata, + unsigned int odr, bool force); + +static int st_lsm6dsm_i2c_master_write(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock); +static int st_lsm6dsm_i2c_master_read(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset); + +#ifdef ST_LSM6DSM_EXT0_HAS_SELFTEST +static ssize_t st_lsm6dsm_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_lsm6dsm_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf); +static ssize_t st_lsm6dsm_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); +#endif /* ST_LSM6DSM_EXT0_HAS_SELFTEST */ + +static ssize_t st_lsm6dsm_i2c_master_sysfs_sampling_frequency_avail( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%d %d %d %d\n", 13, 26, 52, 104); +} + +static ssize_t st_lsm6dsm_i2c_master_sysfs_get_sampling_frequency( + struct device *dev, struct device_attribute *attr, char *buf) +{ + struct lsm6dsm_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->cdata->v_odr[sdata->sindex]); +} + +static ssize_t st_lsm6dsm_i2c_master_sysfs_set_sampling_frequency( + struct device *dev, struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + mutex_lock(&sdata->cdata->odr_lock); + + if (sdata->cdata->v_odr[sdata->sindex] != odr) + err = st_lsm6dsm_i2c_master_set_odr(sdata, odr, false); + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_lsm6dsm_i2c_master_sysfs_get_sampling_frequency, + st_lsm6dsm_i2c_master_sysfs_set_sampling_frequency); + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL( + st_lsm6dsm_i2c_master_sysfs_sampling_frequency_avail); + +static ST_LSM6DSM_HWFIFO_ENABLED(); +static ST_LSM6DSM_HWFIFO_WATERMARK(); +static ST_LSM6DSM_HWFIFO_WATERMARK_MIN(); +static ST_LSM6DSM_HWFIFO_WATERMARK_MAX(); +static ST_LSM6DSM_HWFIFO_FLUSH(); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsm_get_module_id, NULL, 0); + +#ifdef ST_LSM6DSM_EXT0_HAS_SELFTEST +static IIO_DEVICE_ATTR(selftest_available, S_IRUGO, + st_lsm6dsm_i2c_master_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(selftest, S_IWUSR | S_IRUGO, + st_lsm6dsm_i2c_master_sysfs_get_selftest_status, + st_lsm6dsm_i2c_master_sysfs_start_selftest, 0); +#endif /* ST_LSM6DSM_EXT0_HAS_SELFTEST */ + +static struct attribute *st_lsm6dsm_ext0_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hwfifo_enabled.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + +#ifdef ST_LSM6DSM_EXT0_HAS_SELFTEST + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, +#endif /* ST_LSM6DSM_EXT0_HAS_SELFTEST */ + + NULL, +}; + +static const struct attribute_group st_lsm6dsm_ext0_attribute_group = { + .attrs = st_lsm6dsm_ext0_attributes, +}; + +static const struct iio_info st_lsm6dsm_ext0_info = { + .attrs = &st_lsm6dsm_ext0_attribute_group, + .read_raw = &st_lsm6dsm_i2c_master_read_raw, +}; + +struct st_lsm6dsm_iio_info_data { + char suffix_name[20]; + struct iio_info *info; + struct iio_chan_spec *channels; + int num_channels; +}; + +struct st_lsm6dsm_reg { + u8 addr; + u8 mask; + u8 def_value; +}; + +struct st_lsm6dsm_power_reg { + u8 addr; + u8 mask; + u8 off_value; + u8 on_value; + bool isodr; +}; + +struct st_lsm6dsm_custom_function { + int (*boot_initialization)(struct lsm6dsm_sensor_data *sdata); +}; + +static struct st_lsm6dsm_exs_list { + struct st_lsm6dsm_reg wai; + struct st_lsm6dsm_reg reset; + struct st_lsm6dsm_reg fullscale; + struct st_lsm6dsm_i2c_master_odr_table odr; + struct st_lsm6dsm_power_reg power; + u8 fullscale_value; + u8 samples_to_discard; + u8 read_data_len; + u8 num_data_channels; + bool available; + unsigned int gain; + u8 i2c_addr; + struct st_lsm6dsm_iio_info_data data; + struct st_lsm6dsm_custom_function cf; +} st_lsm6dsm_exs_list[] = { + { + .wai = { + .addr = ST_LSM6DSM_EXT0_WAI_ADDR, + .def_value = ST_LSM6DSM_EXT0_WAI_VALUE, + }, + .reset = { + .addr = ST_LSM6DSM_EXT0_RESET_ADDR, + .mask = ST_LSM6DSM_EXT0_RESET_MASK, + }, +#ifdef ST_LSM6DSM_EXT0_HAS_FULLSCALE + .fullscale = { + .addr = ST_LSM6DSM_EXT0_FULLSCALE_ADDR, + .mask = ST_LSM6DSM_EXT0_FULLSCALE_MASK, + .def_value = ST_LSM6DSM_EXT0_FULLSCALE_VALUE, + }, +#endif + .odr = { + .addr = ST_LSM6DSM_EXT0_ODR_ADDR, + .mask = ST_LSM6DSM_EXT0_ODR_MASK, + .odr_avl = { + { + .hz = ST_LSM6DSM_EXT0_ODR0_HZ, + .value = ST_LSM6DSM_EXT0_ODR0_VALUE, + }, + { + .hz = ST_LSM6DSM_EXT0_ODR1_HZ, + .value = ST_LSM6DSM_EXT0_ODR1_VALUE, + }, + { + .hz = ST_LSM6DSM_EXT0_ODR2_HZ, + .value = ST_LSM6DSM_EXT0_ODR2_VALUE, + }, + { + .hz = ST_LSM6DSM_EXT0_ODR3_HZ, + .value = ST_LSM6DSM_EXT0_ODR3_VALUE, + }, + }, + }, + .power = { + .addr = ST_LSM6DSM_EXT0_PW_ADDR, + .mask = ST_LSM6DSM_EXT0_PW_MASK, + .off_value = ST_LSM6DSM_EXT0_PW_OFF, + .on_value = ST_LSM6DSM_EXT0_PW_ON, + }, + .samples_to_discard = ST_LSM6DSM_EXT0_STD, + .read_data_len = ST_LSM6DSM_EXT0_READ_DATA_LEN, + .num_data_channels = 3, + .available = false, + .gain = ST_LSM6DSM_EXT0_GAIN_VALUE, + .i2c_addr = ST_LSM6DSM_EXT0_ADDR, + .data = { + .suffix_name = ST_LSM6DSM_EXT0_SUFFIX_NAME, + .info = (struct iio_info *)&st_lsm6dsm_ext0_info, + .channels = (struct iio_chan_spec *)&st_lsm6dsm_ext0_ch, + .num_channels = ARRAY_SIZE(st_lsm6dsm_ext0_ch), + }, + .cf.boot_initialization = ST_LSM6DSM_EXT0_BOOT_FUNCTION, + } +}; + +static inline void st_lsm6dsm_master_wait_completed(struct lsm6dsm_data *cdata) +{ + msleep((1000U / cdata->trigger_odr) + 2); +} + +static int st_lsm6dsm_i2c_master_read(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, + bool transfer_lock, bool read_status_end, u8 offset) +{ + int err; + u8 slave_conf[3]; + + slave_conf[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | + ST_LSM6DSM_SLVX_READ; + slave_conf[1] = reg_addr; + slave_conf[2] = (len & 0x07); + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + + if (en_sensor_hub) { + err = st_lsm6dsm_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + + st_lsm6dsm_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_LSM6DSM_SLV0_OUT_ADDR + + offset, len & 0x07, data, true); + if (err < 0) + goto i2c_master_read_unlock_mutex; + +#ifdef ST_LSM6DSM_EXT0_IS_AKM + if (read_status_end) { + slave_conf[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + slave_conf[1] = ST_LSM6DSM_EXT0_DATA_STATUS; + slave_conf[2] = 0x01; + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_SLV2_ADDR_ADDR, slave_conf, + ARRAY_SIZE(slave_conf)); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } +#endif /* ST_LSM6DSM_EXT0_IS_AKM */ + + if (en_sensor_hub) { + err = st_lsm6dsm_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_read_unlock_mutex; + } + +i2c_master_read_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len & 0x07; +} + +static int st_lsm6dsm_i2c_master_write(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool en_sensor_hub, bool transfer_lock) +{ + int err, i = 0; + u8 slave0_conf[2]; + + if (transfer_lock) + mutex_lock(&cdata->i2c_transfer_lock); + + while (i < len) { + slave0_conf[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1); + slave0_conf[1] = reg_addr + i; + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + slave0_conf[0] = data[i]; + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_DATAWRITE_SLV0, + slave0_conf, 1); + if (err < 0) + goto i2c_master_write_unlock_mutex; + + if (en_sensor_hub) { + err = st_lsm6dsm_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + st_lsm6dsm_master_wait_completed(cdata); + + if (en_sensor_hub) { + err = st_lsm6dsm_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + goto i2c_master_write_unlock_mutex; + } + + i++; + } + + slave0_conf[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_LSM6DSM_EN_BIT; + slave0_conf[1] = st_lsm6dsm_exs_list[EXT0_INDEX].wai.addr; + + st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_SLV0_ADDR_ADDR, + slave0_conf, + ARRAY_SIZE(slave0_conf)); + +i2c_master_write_unlock_mutex: + if (transfer_lock) + mutex_unlock(&cdata->i2c_transfer_lock); + + return err < 0 ? err : len; +} + +static int st_lsm6dsm_i2c_master_write_data_with_mask( + struct lsm6dsm_data *cdata, u8 reg_addr, u8 mask, u8 data) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + mutex_lock(&cdata->i2c_transfer_lock); + disable_irq(cdata->irq); + + err = st_lsm6dsm_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + err = st_lsm6dsm_i2c_master_read(cdata, reg_addr, 1, + &old_data, false, false, true, + st_lsm6dsm_exs_list[0].read_data_len); + if (err < 0) { + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + return err; + } + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data != old_data) + err = st_lsm6dsm_i2c_master_write(cdata, reg_addr, + 1, &new_data, false, false); + + st_lsm6dsm_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + + enable_irq(cdata->irq); + mutex_unlock(&cdata->i2c_transfer_lock); + + return err; +} + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6dsm_sensor_data *sdata) +{ + + return st_lsm6dsm_i2c_master_write_data_with_mask( + sdata->cdata, + ST_LSM6DSM_EXT0_BDU_ADDR, + ST_LSM6DSM_EXT0_BDU_MASK, ST_LSM6DSM_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911 +static int akm09911_initialization(struct lsm6dsm_sensor_data *sdata) +{ + int err; u8 data[ST_LSM6DSM_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6dsm_i2c_master_read(sdata->cdata, + ST_LSM6DSM_EXT0_SENSITIVITY_ADDR, + ST_LSM6DSM_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_lsm6dsm_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0]) * 1000) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1]) * 1000) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2]) * 1000) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911 */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6dsm_sensor_data *sdata) +{ + int err; u8 data[ST_LSM6DSM_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6dsm_i2c_master_read(sdata->cdata, + ST_LSM6DSM_EXT0_SENSITIVITY_ADDR, + ST_LSM6DSM_EXT0_SENSITIVITY_LEN, + data, true, true, false, + st_lsm6dsm_exs_list[0].read_data_len); + if (err < 0) + return err; + + /* gain expressed in nT/LSB */ + sdata->c_gain[0] = (((((int)data[0] - 128) * 500) >> 7) + 1000); + sdata->c_gain[1] = (((((int)data[1] - 128) * 500) >> 7) + 1000); + sdata->c_gain[2] = (((((int)data[2] - 128) * 500) >> 7) + 1000); + + /* gain expressed in G/LSB */ + sdata->c_gain[0] *= 10; + sdata->c_gain[1] *= 10; + sdata->c_gain[2] *= 10; + + return 0; +} +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB +static int lps22hb_initialization(struct lsm6dsm_sensor_data *sdata) +{ + + return st_lsm6dsm_i2c_master_write_data_with_mask( + sdata->cdata, + ST_LSM6DSM_EXT0_BDU_ADDR, + ST_LSM6DSM_EXT0_BDU_MASK, ST_LSM6DSM_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LPS22HB */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS2MDL +static int lis2mdl_initialization(struct lsm6dsm_sensor_data *sdata) +{ + int err; + + err = st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_EXT0_TEMP_COMP_ADDR, + ST_LSM6DSM_EXT0_TEMP_COMP_MASK, + 1); + if (err < 0) + return err; + + err = st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_EXT0_OFF_CANC_ADDR, + ST_LSM6DSM_EXT0_OFF_CANC_MASK, + 1); + if (err < 0) + return err; + + return st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + ST_LSM6DSM_EXT0_BDU_ADDR, + ST_LSM6DSM_EXT0_BDU_MASK, + ST_LSM6DSM_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS2MDL */ + +#ifdef ST_LSM6DSM_EXT0_HAS_SELFTEST +static ssize_t st_lsm6dsm_i2c_master_sysfs_get_selftest_available( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "absolute\n"); +} + +static ssize_t st_lsm6dsm_i2c_master_sysfs_get_selftest_status( + struct device *dev, struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + result = sdata->cdata->ext0_selftest_status; + mutex_unlock(&sdata->cdata->odr_lock); + + if (result == 0) + message = ST_LSM6DSM_SELFTEST_NA_MS; + else if (result < 0) + message = ST_LSM6DSM_SELFTEST_FAIL_MS; + else if (result > 0) + message = ST_LSM6DSM_SELFTEST_PASS_MS; + + return sprintf(buf, "%s\n", message); +} + +static ssize_t st_lsm6dsm_i2c_master_sysfs_start_selftest(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + int x_selftest = 0, y_selftest = 0, z_selftest = 0; + u8 outdata[8], reg_addr, reg_status = 0, temp_reg_status; +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL + int i, x = 0, y = 0, z = 0; + u8 reg_status2 = 0, reg_status3 = 0; + u8 reg_addr2, reg_addr3, temp_reg_status2, temp_reg_status3; +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ +#ifdef ST_LSM6DSM_EXT0_IS_AKM + u8 temp, sh_config[3], timeout = 0; +#endif /* ST_LSM6DSM_EXT0_IS_AKM */ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + sdata->cdata->ext0_selftest_status = 0; + + if (sdata->cdata->sensors_enabled > 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EBUSY; + } + + if (strncmp(buf, "absolute", size - 2) != 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return -EINVAL; + } + + err = st_lsm6dsm_enable_sensor_hub(sdata->cdata, true, ST_MASK_ID_EXT0); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL + reg_addr = ST_LSM6DSM_SELFTEST_ADDR1; + temp_reg_status = ST_LSM6DSM_SELFTEST_ADDR1_VALUE; + reg_addr2 = ST_LSM6DSM_SELFTEST_ADDR2; + temp_reg_status2 = ST_LSM6DSM_SELFTEST_ADDR2_VALUE; + reg_addr3 = ST_LSM6DSM_SELFTEST_ADDR3; + temp_reg_status3 = ST_LSM6DSM_SELFTEST_ADDR3_VALUE; +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ + +#ifdef ST_LSM6DSM_EXT0_IS_AKM + reg_addr = ST_LSM6DSM_SELFTEST_ADDR; + temp_reg_status = ST_LSM6DSM_SELFTEST_ENABLE; +#endif /* ST_LSM6DSM_EXT0_IS_AKM */ + + err = st_lsm6dsm_i2c_master_read(sdata->cdata, reg_addr, 1, + ®_status, false, true, false, + st_lsm6dsm_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + +#ifdef ST_LSM6DSM_EXT0_IS_AKM + /* SLAVE 1 is disabled for a while, dummy write to wai reg */ + sh_config[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6dsm_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; + + /* SLAVE 2 is disabled for a while, dummy read of wai reg */ + sh_config[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6dsm_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 1; + + err = st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_SLV2_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto disable_sensor_hub; +#endif /* ST_LSM6DSM_EXT0_IS_AKM */ + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL + err = st_lsm6dsm_i2c_master_read(sdata->cdata, reg_addr2, 1, + ®_status2, false, true, false, + st_lsm6dsm_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; + + err = st_lsm6dsm_i2c_master_read(sdata->cdata, reg_addr3, 1, + ®_status3, false, true, false, + st_lsm6dsm_exs_list[0].read_data_len); + if (err < 0) + goto disable_sensor_hub; +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto disable_sensor_hub; + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr2, 1, + &temp_reg_status2, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr3, 1, + &temp_reg_status3, false, true); + if (err < 0) + goto restore_status_reg2; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_lsm6dsm_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x += ((s16)*(u16 *)&outdata[0]) / 10; + y += ((s16)*(u16 *)&outdata[2]) / 10; + z += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + temp_reg_status = ST_LSM6DSM_SELFTEST_ENABLE; + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr, 1, + &temp_reg_status, false, true); + if (err < 0) + goto restore_status_reg3; + + /* get data with selftest disabled */ + msleep(100); + + for (i = 0; i < 10; i++) { + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + st_lsm6dsm_exs_list[0].read_data_len, outdata, true); + if (err < 0) { + i--; + continue; + } + + x_selftest += ((s16)*(u16 *)&outdata[0]) / 10; + y_selftest += ((s16)*(u16 *)&outdata[2]) / 10; + z_selftest += ((s16)*(u16 *)&outdata[4]) / 10; + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + } + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); + if (err < 0) + goto restore_status_reg3; + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); + if (err < 0) + goto restore_status_reg2; + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6dsm_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + if ((abs(x_selftest - x) < ST_LSM6DSM_SELFTEST_EXT0_MIN) || + (abs(x_selftest - x) > ST_LSM6DSM_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(y_selftest - y) < ST_LSM6DSM_SELFTEST_EXT0_MIN) || + (abs(y_selftest - y) > ST_LSM6DSM_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((abs(z_selftest - z) < ST_LSM6DSM_SELFTEST_EXT0_MIN_Z) || + (abs(z_selftest - z) > ST_LSM6DSM_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ + +#ifdef ST_LSM6DSM_EXT0_IS_AKM + do { + msleep(1000U / sdata->cdata->trigger_odr); + + err = st_lsm6dsm_i2c_master_read(sdata->cdata, + ST_LSM6DSM_SELFTEST_STATUS_REG, 1, + &temp, false, true, false, 1); + if (err < 0) + goto restore_status_reg; + + timeout++; + } while (((temp & 0x01) == 0) && (timeout < 5)); + + if (timeout >= 5) { + err = -EINVAL; + goto restore_status_reg; + } + + err = st_lsm6dsm_i2c_master_read(sdata->cdata, + st_lsm6dsm_exs_list[0].data.channels[0].address, + st_lsm6dsm_exs_list[0].read_data_len, + outdata, false, true, true, 1); + if (err < 0) + goto restore_status_reg; + +#ifdef ST_LSM6DSM_EXT0_IS_AKM + /* SLAVE 2 recovering */ + sh_config[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | 0x01; + sh_config[1] = st_lsm6dsm_exs_list[0].data.channels[0].address; + sh_config[2] = st_lsm6dsm_exs_list[0].read_data_len; + + err = st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + goto restore_status_reg; +#endif /* ST_LSM6DSM_EXT0_IS_AKM */ + + err = st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); + if (err < 0) + goto restore_status_reg; + + err = st_lsm6dsm_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + if (err < 0) + goto disable_sensor_hub; + + x_selftest = ((s16)*(u16 *)&outdata[0]); + y_selftest = ((s16)*(u16 *)&outdata[2]); + z_selftest = ((s16)*(u16 *)&outdata[4]); + +#if defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09912) || \ + defined(CONFIG_ST_LSM6DSM_IIO_EXT0_AKM09911) + x_selftest *= sdata->c_gain[0]; + y_selftest *= sdata->c_gain[1]; + z_selftest *= sdata->c_gain[2]; + + x_selftest /= 10000; + y_selftest /= 10000; + z_selftest /= 10000; +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_AKM0991X */ + + if ((x_selftest < ST_LSM6DSM_SELFTEST_EXT0_MIN) || + (x_selftest > ST_LSM6DSM_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((y_selftest < ST_LSM6DSM_SELFTEST_EXT0_MIN) || + (y_selftest > ST_LSM6DSM_SELFTEST_EXT0_MAX)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } + + if ((z_selftest < ST_LSM6DSM_SELFTEST_EXT0_MIN_Z) || + (z_selftest > ST_LSM6DSM_SELFTEST_EXT0_MAX_Z)) { + sdata->cdata->ext0_selftest_status = -1; + mutex_unlock(&sdata->cdata->odr_lock); + return size; + } +#endif /* ST_LSM6DSM_EXT0_IS_AKM */ + + sdata->cdata->ext0_selftest_status = 1; + + mutex_unlock(&sdata->cdata->odr_lock); + + return size; + +#ifdef CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL +restore_status_reg3: + st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr3, 1, + ®_status3, false, true); +restore_status_reg2: + st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr2, 1, + ®_status2, false, true); +#endif /* CONFIG_ST_LSM6DSM_IIO_EXT0_LIS3MDL */ +restore_status_reg: + st_lsm6dsm_i2c_master_write(sdata->cdata, reg_addr, 1, + ®_status, false, true); +disable_sensor_hub: + st_lsm6dsm_enable_sensor_hub(sdata->cdata, false, ST_MASK_ID_EXT0); + mutex_unlock(&sdata->cdata->odr_lock); + return err; +} +#endif /* ST_LSM6DSM_EXT0_HAS_SELFTEST */ + + +static int st_lsm6dsm_i2c_master_set_odr(struct lsm6dsm_sensor_data *sdata, + unsigned int odr, bool force) +{ + int i, err, err2; + u8 value, mask, addr; + bool scan_odr = true; + unsigned int current_odr = sdata->cdata->v_odr[sdata->sindex]; + unsigned int current_hw_odr = sdata->cdata->hw_odr[sdata->sindex]; + + if (odr == 0) { + if (force) + scan_odr = false; + else + return -EINVAL; + } + if (scan_odr) { + switch (odr) { + case 13: + case 26: + case 52: + case 104: + break; + default: + return -EINVAL; + } + + for (i = 0; i < ST_LSM6DSM_ODR_LIST_NUM; i++) { + if (st_lsm6dsm_exs_list[0].odr.odr_avl[i].hz >= odr) + break; + } + if (i == ST_LSM6DSM_ODR_LIST_NUM) + i--; + + if (!force) { + if ((sdata->cdata->sensors_enabled & BIT(sdata->sindex)) == 0) { + sdata->cdata->v_odr[sdata->sindex] = odr; + return 0; + } + } + + addr = st_lsm6dsm_exs_list[0].odr.addr; + mask = st_lsm6dsm_exs_list[0].odr.mask; + value = st_lsm6dsm_exs_list[0].odr.odr_avl[i].value; + } else { + if (st_lsm6dsm_exs_list[0].power.isodr) { + addr = st_lsm6dsm_exs_list[0].power.addr; + mask = st_lsm6dsm_exs_list[0].power.mask; + value = st_lsm6dsm_exs_list[0].power.off_value; + } else + goto skip_i2c_write; + } + + sdata->cdata->samples_to_discard[ST_MASK_ID_EXT0] = + st_lsm6dsm_exs_list[0].samples_to_discard; + + err = st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + addr, mask, value); + if (err < 0) + return err; + +skip_i2c_write: + if (odr == 0) + sdata->cdata->hw_odr[sdata->sindex] = 0; + else + sdata->cdata->hw_odr[sdata->sindex] = odr; + + if (!force) { + sdata->cdata->v_odr[sdata->sindex] = odr; + + err = st_lsm6dsm_enable_sensor_hub(sdata->cdata, + true, ST_MASK_ID_EXT0); + if (err < 0) { + sdata->cdata->hw_odr[sdata->sindex] = current_hw_odr; + sdata->cdata->v_odr[sdata->sindex] = current_odr; + do { + err2 = st_lsm6dsm_enable_sensor_hub(sdata->cdata, + false, ST_MASK_ID_EXT0); + msleep(200); + } while (err2 < 0); + + return err; + } + } + + return 0; +} + +static int st_lsm6dsm_i2c_master_set_enable( + struct lsm6dsm_sensor_data *sdata, bool enable, bool buffer) +{ + int err; + u8 reg_value; + + /* If odr != power this part should enable/disable sensor */ + if (!st_lsm6dsm_exs_list[0].power.isodr) { + if (enable) + reg_value = st_lsm6dsm_exs_list[0].power.on_value; + else + reg_value = st_lsm6dsm_exs_list[0].power.off_value; + + err = st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6dsm_exs_list[0].power.addr, + st_lsm6dsm_exs_list[0].power.mask, + reg_value); + if (err < 0) + return err; + } + + err = st_lsm6dsm_enable_sensor_hub(sdata->cdata, + enable, ST_MASK_ID_EXT0); + if (err < 0) + return err; + + err = st_lsm6dsm_i2c_master_set_odr(sdata, + enable ? sdata->cdata->v_odr[sdata->sindex] : 0, true); + if (err < 0) + goto disable_sensorhub; + + if (buffer) { + err = st_lsm6dsm_set_drdy_irq(sdata, enable); + if (err < 0) + goto restore_odr; + + if (enable) + sdata->cdata->sensors_enabled |= BIT(sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~BIT(sdata->sindex); + } + + return 0; + +restore_odr: + st_lsm6dsm_i2c_master_set_odr(sdata, + enable ? 0 : sdata->cdata->v_odr[sdata->sindex], true); +disable_sensorhub: + st_lsm6dsm_enable_sensor_hub(sdata->cdata, !enable, ST_MASK_ID_EXT0); + + return err; +} + +static int st_lsm6dsm_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + int err, ch_num_byte = ch->scan_type.storagebits >> 3; + u8 outdata[4]; + + if (ch_num_byte > ARRAY_SIZE(outdata)) + return -ENOMEM; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6dsm_i2c_master_set_enable(sdata, true, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + st_lsm6dsm_master_wait_completed(sdata->cdata); + + msleep((1000U / sdata->cdata->trigger_odr) + 2); + + err = sdata->cdata->tf->read(sdata->cdata, sdata->data_out_reg, + ch_num_byte, outdata, true); + if (err < 0) { + st_lsm6dsm_i2c_master_set_enable(sdata, false, false); + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + err = st_lsm6dsm_i2c_master_set_enable(sdata, false, false); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + if (ch_num_byte > 2) + *val = (s32)get_unaligned_le32(outdata); + else + *val = (s16)get_unaligned_le16(outdata); + + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&sdata->cdata->odr_lock); + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[ch->scan_index]; + + if (ch->type == IIO_TEMP) { + *val = 1; + *val2 = 0; + return IIO_VAL_INT; + } + + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6dsm_i2c_master_buffer_preenable(struct iio_dev *indio_dev) +{ +#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + if (sdata->cdata->injection_mode) + return -EBUSY; +#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */ + + return 0; +} + +static int st_lsm6dsm_i2c_master_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + sdata->cdata->fifo_output[sdata->sindex].initialized = false; + + if ((sdata->cdata->hwfifo_enabled[ST_MASK_ID_EXT0]) && + (indio_dev->buffer->length < 2 * ST_LSM6DSM_MAX_FIFO_LENGHT)) + return -EINVAL; + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6dsm_i2c_master_set_enable(sdata, true, true); + if (err < 0) { + mutex_unlock(&sdata->cdata->odr_lock); + return err; + } + + mutex_unlock(&sdata->cdata->odr_lock); + + return 0; +} + +static int st_lsm6dsm_i2c_master_buffer_postdisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->odr_lock); + + err = st_lsm6dsm_i2c_master_set_enable(sdata, false, true); + + mutex_unlock(&sdata->cdata->odr_lock); + + return err < 0 ? err : 0; +} + +static const struct iio_trigger_ops st_lsm6dsm_i2c_master_trigger_ops = { + .set_trigger_state = &st_lsm6dsm_trig_set_state, +}; + +int st_lsm6dsm_i2c_master_allocate_trigger(struct lsm6dsm_data *cdata) +{ + int err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[ST_MASK_ID_EXT0] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[ST_MASK_ID_EXT0]->name); +#endif /* LINUX_VERSION_CODE */ + + + if (!cdata->trig[ST_MASK_ID_EXT0]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + return -ENOMEM; + } + + iio_trigger_set_drvdata(cdata->trig[ST_MASK_ID_EXT0], + cdata->indio_dev[ST_MASK_ID_EXT0]); + cdata->trig[ST_MASK_ID_EXT0]->ops = &st_lsm6dsm_i2c_master_trigger_ops; + cdata->trig[ST_MASK_ID_EXT0]->dev.parent = cdata->dev; + + err = iio_trigger_register(cdata->trig[ST_MASK_ID_EXT0]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + goto deallocate_trigger; + } + + cdata->indio_dev[ST_MASK_ID_EXT0]->trig = cdata->trig[ST_MASK_ID_EXT0]; + + return 0; + +deallocate_trigger: + iio_trigger_free(cdata->trig[ST_MASK_ID_EXT0]); + return err; +} + +static void st_lsm6dsm_i2c_master_deallocate_trigger(struct lsm6dsm_data *cdata) +{ + iio_trigger_unregister(cdata->trig[ST_MASK_ID_EXT0]); +} + +static const struct iio_buffer_setup_ops st_lsm6dsm_i2c_master_buffer_setup_ops = { + .preenable = &st_lsm6dsm_i2c_master_buffer_preenable, + .postenable = &st_lsm6dsm_i2c_master_buffer_postenable, + .postdisable = &st_lsm6dsm_i2c_master_buffer_postdisable, +}; + +static inline irqreturn_t st_lsm6dsm_i2c_master_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +static int st_lsm6dsm_i2c_master_allocate_buffer(struct lsm6dsm_data *cdata) +{ + return iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_EXT0], + &st_lsm6dsm_i2c_master_handler_empty, NULL, + &st_lsm6dsm_i2c_master_buffer_setup_ops); +} + +static void st_lsm6dsm_i2c_master_deallocate_buffer(struct lsm6dsm_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_EXT0]); +} + +static int st_lsm6dsm_i2c_master_send_sensor_hub_parameters( + struct lsm6dsm_sensor_data *sdata) +{ + int err; + u8 sh_config[3]; + + /* SLAVE 0 is used by write */ + sh_config[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_LSM6DSM_EN_BIT; + sh_config[1] = st_lsm6dsm_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x20; + + err = st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + /* SLAVE 1 is used to read output data */ + sh_config[0] = (st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr << 1) | ST_LSM6DSM_EN_BIT; + sh_config[1] = st_lsm6dsm_exs_list[0].data.channels[0].address; + sh_config[2] = st_lsm6dsm_exs_list[0].read_data_len; + + err = st_lsm6dsm_write_embedded_registers(sdata->cdata, + ST_LSM6DSM_SLV1_ADDR_ADDR, + sh_config, ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6dsm_i2c_master_init_sensor(struct lsm6dsm_sensor_data *sdata) +{ + int err, ext_num = 0; + + err = st_lsm6dsm_i2c_master_send_sensor_hub_parameters(sdata); + if (err < 0) + return err; + + sdata->c_gain[0] = st_lsm6dsm_exs_list[ext_num].gain; + sdata->c_gain[1] = st_lsm6dsm_exs_list[ext_num].gain; + sdata->c_gain[2] = st_lsm6dsm_exs_list[ext_num].gain; + + if ((st_lsm6dsm_exs_list[ext_num].power.addr == + st_lsm6dsm_exs_list[ext_num].odr.addr) && + (st_lsm6dsm_exs_list[ext_num].power.mask == + st_lsm6dsm_exs_list[ext_num].odr.mask)) + st_lsm6dsm_exs_list[ext_num].power.isodr = true; + else + st_lsm6dsm_exs_list[ext_num].power.isodr = false; + + err = st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6dsm_exs_list[ext_num].reset.addr, + st_lsm6dsm_exs_list[ext_num].reset.mask, + ST_LSM6DSM_EN_BIT); + if (err < 0) + return err; + + usleep_range(200, 1000); + + if (st_lsm6dsm_exs_list[ext_num].fullscale.addr > 0) { + err = st_lsm6dsm_i2c_master_write_data_with_mask(sdata->cdata, + st_lsm6dsm_exs_list[ext_num].fullscale.addr, + st_lsm6dsm_exs_list[ext_num].fullscale.mask, + st_lsm6dsm_exs_list[ext_num].fullscale.def_value); + if (err < 0) + return err; + } + + if (st_lsm6dsm_exs_list[0].cf.boot_initialization != NULL) { + err = st_lsm6dsm_exs_list[0].cf.boot_initialization(sdata); + if (err < 0) + return err; + } + + err = st_lsm6dsm_i2c_master_set_enable(sdata, false, false); + if (err < 0) + return err; + + return 0; +} + +static int st_lsm6dsm_i2c_master_allocate_device(struct lsm6dsm_data *cdata) +{ + int err; + struct lsm6dsm_sensor_data *sdata_ext; + + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + + sdata_ext->num_data_channels = + st_lsm6dsm_exs_list[0].num_data_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->name = kasprintf(GFP_KERNEL, + "%s_%s", cdata->name, + st_lsm6dsm_exs_list[0].data.suffix_name); + + cdata->indio_dev[ST_MASK_ID_EXT0]->info = + st_lsm6dsm_exs_list[0].data.info; + cdata->indio_dev[ST_MASK_ID_EXT0]->channels = + st_lsm6dsm_exs_list[0].data.channels; + cdata->indio_dev[ST_MASK_ID_EXT0]->num_channels = + st_lsm6dsm_exs_list[0].data.num_channels; + + cdata->indio_dev[ST_MASK_ID_EXT0]->modes = INDIO_DIRECT_MODE; + + sdata_ext->data_out_reg = ST_LSM6DSM_SLV0_OUT_ADDR; + + err = st_lsm6dsm_i2c_master_init_sensor(sdata_ext); + if (err < 0) + return err; + + err = st_lsm6dsm_i2c_master_allocate_buffer(cdata); + if (err < 0) + return err; + + err = st_lsm6dsm_i2c_master_allocate_trigger(cdata); + if (err < 0) + goto iio_deallocate_buffer; + + err = iio_device_register(cdata->indio_dev[ST_MASK_ID_EXT0]); + if (err < 0) + goto iio_deallocate_trigger; + + return 0; + +iio_deallocate_trigger: + st_lsm6dsm_i2c_master_deallocate_trigger(cdata); +iio_deallocate_buffer: + st_lsm6dsm_i2c_master_deallocate_buffer(cdata); + return err; +} + +static void st_lsm6dsm_i2c_master_deallocate_device(struct lsm6dsm_data *cdata) +{ + iio_device_unregister(cdata->indio_dev[ST_MASK_ID_EXT0]); + st_lsm6dsm_i2c_master_deallocate_trigger(cdata); + st_lsm6dsm_i2c_master_deallocate_buffer(cdata); +} + +int st_lsm6dsm_i2c_master_probe(struct lsm6dsm_data *cdata) +{ + int err, i; + u8 sh_config[3]; + u8 wai, i2c_address; + struct lsm6dsm_sensor_data *sdata_ext; + + mutex_init(&cdata->i2c_transfer_lock); + cdata->v_odr[ST_MASK_ID_EXT0] = 13; + cdata->ext0_available = false; + cdata->ext0_selftest_status = false; + +#ifdef CONFIG_ST_LSM6DSM_ENABLE_INTERNAL_PULLUP + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_INTER_PULLUP_ADDR, + ST_LSM6DSM_INTER_PULLUP_MASK, + ST_LSM6DSM_EN_BIT, true); + if (err < 0) + return err; +#endif /* CONFIG_ST_LSM6DSM_ENABLE_INTERNAL_PULLUP */ + + err = st_lsm6dsm_write_data_with_mask(cdata, + ST_LSM6DSM_FUNC_MAX_RATE_ADDR, + ST_LSM6DSM_FUNC_MAX_RATE_MASK, 1, true); + if (err < 0) + return err; + + cdata->indio_dev[ST_MASK_ID_EXT0] = devm_iio_device_alloc(cdata->dev, + sizeof(*sdata_ext)); + if (!cdata->indio_dev[ST_MASK_ID_EXT0]) + return -ENOMEM; + + sdata_ext = iio_priv(cdata->indio_dev[ST_MASK_ID_EXT0]); + sdata_ext->cdata = cdata; + sdata_ext->sindex = ST_MASK_ID_EXT0; + cdata->samples_to_discard_2[ST_MASK_ID_EXT0] = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].sip = 0; + sdata_ext->cdata->fifo_output[ST_MASK_ID_EXT0].timestamp_p = 0; + + for (i = 0; i < 2; i++) { + if (i == 0) + i2c_address = ST_LSM6DSM_EXT0_ADDR; + else + i2c_address = ST_LSM6DSM_EXT0_ADDR2; + + /* to check if sensor is available use SLAVE0 first time */ + sh_config[0] = (i2c_address << 1) | 0x01; + sh_config[1] = st_lsm6dsm_exs_list[EXT0_INDEX].wai.addr; + sh_config[2] = 0x01; + + err = st_lsm6dsm_write_embedded_registers(cdata, + ST_LSM6DSM_SLV0_ADDR_ADDR, sh_config, + ARRAY_SIZE(sh_config)); + if (err < 0) + return err; + + err = st_lsm6dsm_enable_sensor_hub(cdata, true, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + msleep(100); + + st_lsm6dsm_master_wait_completed(cdata); + + err = cdata->tf->read(cdata, ST_LSM6DSM_SLV0_OUT_ADDR, + 1, &wai, true); + if (err < 0) { + err = st_lsm6dsm_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + continue; + } + + err = st_lsm6dsm_enable_sensor_hub(cdata, false, + ST_MASK_ID_SENSOR_HUB_ASYNC_OP); + if (err < 0) + return err; + + st_lsm6dsm_exs_list[EXT0_INDEX].i2c_addr = i2c_address; + break; + } + if (i == 2) + goto ext0_sensor_not_available; + + /* after wai check SLAVE0 is used for write, SLAVE1 for async read + and SLAVE2 to read sensor output data */ + + if (wai != st_lsm6dsm_exs_list[EXT0_INDEX].wai.def_value) { + dev_err(cdata->dev, "wai value of external sensor 0 mismatch\n"); + return err; + } + + err = st_lsm6dsm_i2c_master_allocate_device(cdata); + if (err < 0) + return err; + + cdata->ext0_available = true; + + return 0; + +ext0_sensor_not_available: + dev_err(cdata->dev, "external sensor 0 not available\n"); + + return err; +} +EXPORT_SYMBOL(st_lsm6dsm_i2c_master_probe); + +int st_lsm6dsm_i2c_master_exit(struct lsm6dsm_data *cdata) +{ + if (cdata->ext0_available) + st_lsm6dsm_i2c_master_deallocate_device(cdata); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsm_i2c_master_exit); diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_spi.c b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_spi.c new file mode 100644 index 000000000000..1fbf844aa001 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_spi.c @@ -0,0 +1,198 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6dsm spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsm.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lsm6dsm_spi_read(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int st_lsm6dsm_spi_write(struct lsm6dsm_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_LSM6DSM_RX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static const struct st_lsm6dsm_transfer_function st_lsm6dsm_tf_spi = { + .write = st_lsm6dsm_spi_write, + .read = st_lsm6dsm_spi_read, +}; + +static int st_lsm6dsm_spi_probe(struct spi_device *spi) +{ + int err; + struct lsm6dsm_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + spi_set_drvdata(spi, cdata); + + cdata->tf = &st_lsm6dsm_tf_spi; + + err = st_lsm6dsm_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6dsm_spi_remove(struct spi_device *spi) +{ + struct lsm6dsm_data *cdata = spi_get_drvdata(spi); + + st_lsm6dsm_common_remove(cdata, spi->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_lsm6dsm_suspend(struct device *dev) +{ + struct lsm6dsm_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6dsm_common_suspend(cdata); +} + +static int __maybe_unused st_lsm6dsm_resume(struct device *dev) +{ + struct lsm6dsm_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6dsm_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6dsm_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsm_suspend, st_lsm6dsm_resume) +}; + +#define ST_LSM6DSM_PM_OPS (&st_lsm6dsm_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DSM_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id st_lsm6dsm_id_table[] = { + { LSM6DSM_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsm_id_table); + +#ifdef CONFIG_OF +static const struct of_device_id lsm6dsm_of_match[] = { + { + .compatible = "st,lsm6dsm", + .data = LSM6DSM_DEV_NAME, + }, + { + .compatible = "st,lsm6dsl", + .data = LSM6DSL_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, lsm6dsm_of_match); +#else /* CONFIG_OF */ +#define lsm6dsm_of_match NULL +#endif /* CONFIG_OF */ + +static struct spi_driver st_lsm6dsm_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6dsm-spi", + .pm = ST_LSM6DSM_PM_OPS, + .of_match_table = of_match_ptr(lsm6dsm_of_match), + }, + .probe = st_lsm6dsm_spi_probe, + .remove = st_lsm6dsm_spi_remove, + .id_table = st_lsm6dsm_id_table, +}; +module_spi_driver(st_lsm6dsm_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lsm6dsm spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_trigger.c b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_trigger.c new file mode 100644 index 000000000000..6fecfe90d4ab --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_trigger.c @@ -0,0 +1,244 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lsm6dsm trigger driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsm.h" + +#define ST_LSM6DSM_DIS_BIT 0x00 +#define ST_LSM6DSM_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DSM_SRC2_FUNC_ADDR 0x54 +#define ST_LSM6DSM_WRIST_TILT_IA 0x55 +#define ST_LSM6DSM_WAKE_UP_SRC 0x1b +#define ST_LSM6DSM_TAP_SRC 0x1c +#define ST_LSM6DSM_STAP_EVENT BIT(5) +#define ST_LSM6DSM_DTAP_EVENT BIT(4) + +#define ST_LSM6DSM_FIFO_DATA_AVL_ADDR 0x3b +#define ST_LSM6DSM_ACCEL_DATA_AVL_ADDR 0x1e + +#define ST_LSM6DSM_ACCEL_DATA_AVL 0x01 +#define ST_LSM6DSM_GYRO_DATA_AVL 0x02 +#define ST_LSM6DSM_SRC_STEP_DETECTOR_DATA_AVL 0x10 +#define ST_LSM6DSM_SRC_SIGN_MOTION_DATA_AVL 0x40 +#define ST_LSM6DSM_SRC_TILT_DATA_AVL 0x20 +#define ST_LSM6DSM_SRC_WTILT_DATA_AVL 0x01 +#define ST_LSM6DSM_SRC_STEP_COUNTER_DATA_AVL 0x80 +#define ST_LSM6DSM_SRC_STEP_COUNTER_DATA_OVR 0x08 +#define ST_LSM6DSM_FIFO_DATA_AVL 0x80 +#define ST_LSM6DSM_FIFO_DATA_OVR 0x40 + + +static irqreturn_t lsm6dsm_irq_management(int irq, void *private) +{ + int err; + bool push; + bool force_read_accel = false; + struct lsm6dsm_data *cdata = private; + u8 src_accel_gyro = 0, src_dig_func = 0, src2_dig_func = 0; + u8 src_tap_func = 0; + + cdata->timestamp = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]); + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + (BIT(ST_MASK_ID_ACCEL) | BIT(ST_MASK_ID_GYRO) | + BIT(ST_MASK_ID_EXT0))) { + err = cdata->tf->read(cdata, ST_LSM6DSM_ACCEL_DATA_AVL_ADDR, + 1, &src_accel_gyro, true); + if (err < 0) + goto read_fifo_status; + + if (src_accel_gyro & ST_LSM6DSM_ACCEL_DATA_AVL) { +#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) + & BIT(ST_MASK_ID_EXT0)) { + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples++; + force_read_accel = true; + + if ((cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = 0; + } else { + push = false; + } + + lsm6dsm_read_output_data(cdata, ST_MASK_ID_EXT0, push); + } +#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */ + + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & + BIT(ST_MASK_ID_ACCEL)) { + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples++; + + if ((cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples % + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator) == 0) { + push = true; + cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = 0; + } else { + push = false; + } + + lsm6dsm_read_output_data(cdata, ST_MASK_ID_ACCEL, push); + } else { + if (force_read_accel) + lsm6dsm_read_output_data(cdata, ST_MASK_ID_ACCEL, false); + } + } + + if (src_accel_gyro & ST_LSM6DSM_GYRO_DATA_AVL) { + if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & BIT(ST_MASK_ID_GYRO)) + lsm6dsm_read_output_data(cdata, ST_MASK_ID_GYRO, true); + } + } + +read_fifo_status: + if (cdata->sensors_use_fifo) + st_lsm6dsm_read_fifo(cdata, false); + + err = cdata->tf->read(cdata, ST_LSM6DSM_SRC_FUNC_ADDR, + 1, &src_dig_func, true); + if (err < 0) + goto exit_irq; + + if ((src_dig_func & ST_LSM6DSM_SRC_STEP_DETECTOR_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_STEP_DETECTOR))) { + st_lsm6dsm_push_data_with_timestamp(cdata, + ST_MASK_ID_STEP_DETECTOR, NULL, cdata->timestamp); + } + + if ((src_dig_func & ST_LSM6DSM_SRC_SIGN_MOTION_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_SIGN_MOTION))) { + iio_push_event(cdata->indio_dev[ST_MASK_ID_SIGN_MOTION], + IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->timestamp); + } + + if (src_dig_func & ST_LSM6DSM_SRC_STEP_COUNTER_DATA_OVR) + cdata->num_steps += (1 << 16); + + if (src_dig_func & ST_LSM6DSM_SRC_STEP_COUNTER_DATA_AVL) + iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_STEP_COUNTER]); + + if ((src_dig_func & ST_LSM6DSM_SRC_TILT_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_TILT))) { + st_lsm6dsm_push_data_with_timestamp(cdata, + ST_MASK_ID_TILT, NULL, cdata->timestamp); + } + + err = cdata->tf->read(cdata, ST_LSM6DSM_SRC2_FUNC_ADDR, + 1, &src2_dig_func, true); + if (err < 0) + goto exit_irq; + + if ((src2_dig_func & ST_LSM6DSM_SRC_WTILT_DATA_AVL) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_WTILT))) + iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_WTILT]); + + err = cdata->tf->read(cdata, ST_LSM6DSM_TAP_SRC, + 1, &src_tap_func, true); + if (err < 0) + goto exit_irq; + + if ((src_tap_func & ST_LSM6DSM_STAP_EVENT) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_TAP))) { + iio_push_event(cdata->indio_dev[ST_MASK_ID_TAP], + IIO_UNMOD_EVENT_CODE(IIO_TAP, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING), + cdata->timestamp); + } + + if ((src_tap_func & ST_LSM6DSM_DTAP_EVENT) && + (cdata->sensors_enabled & BIT(ST_MASK_ID_TAP_TAP))) { + iio_push_event(cdata->indio_dev[ST_MASK_ID_TAP_TAP], + IIO_UNMOD_EVENT_CODE(IIO_TAP_TAP, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING), + cdata->timestamp); + } + +exit_irq: + return IRQ_HANDLED; +} + +int st_lsm6dsm_allocate_triggers(struct lsm6dsm_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + cdata->trig[i] = iio_trigger_alloc(cdata->dev, + "%s-trigger", + cdata->indio_dev[i]->name); +#else /* LINUX_VERSION_CODE */ + cdata->trig[i] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[i]->name); +#endif /* LINUX_VERSION_CODE */ + + if (!cdata->trig[i]) { + dev_err(cdata->dev, + "failed to allocate iio trigger.\n"); + err = -ENOMEM; + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]); + cdata->trig[i]->ops = trigger_ops; + cdata->trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, NULL, lsm6dsm_irq_management, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_trigger_register(cdata->trig[n]); + if (err < 0) { + dev_err(cdata->dev, + "failed to register iio trigger.\n"); + goto free_irq; + } + cdata->indio_dev[n]->trig = cdata->trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->trig[i]); + + return err; +} +EXPORT_SYMBOL(st_lsm6dsm_allocate_triggers); + +void st_lsm6dsm_deallocate_triggers(struct lsm6dsm_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_trigger_unregister(cdata->trig[i]); +} +EXPORT_SYMBOL(st_lsm6dsm_deallocate_triggers); diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/Kconfig b/drivers/iio/stm/imu/st_lsm6dso16is/Kconfig new file mode 100644 index 000000000000..e5cf7bcebaba --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/Kconfig @@ -0,0 +1,26 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM6DSO16IS + tristate "STMicroelectronics LSM6DSO16IS sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_LSM6DSO16IS_I2C if (I2C) + select IIO_ST_LSM6DSO16IS_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics + LSM6DSO16IS imu sensors. + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dso16is. + +config IIO_ST_LSM6DSO16IS_I2C + tristate + select REGMAP_I2C + depends on IIO_ST_LSM6DSO16IS + +config IIO_ST_LSM6DSO16IS_SPI + tristate + select REGMAP_SPI + depends on IIO_ST_LSM6DSO16IS + diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/Makefile b/drivers/iio/stm/imu/st_lsm6dso16is/Makefile new file mode 100644 index 000000000000..5282c8806d38 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/Makefile @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_lsm6dso16is-y := st_lsm6dso16is_core.o \ + st_lsm6dso16is_shub.o \ + st_lsm6dso16is_triggers.o + +obj-$(CONFIG_IIO_ST_LSM6DSO16IS) += st_lsm6dso16is.o +obj-$(CONFIG_IIO_ST_LSM6DSO16IS_I2C) += st_lsm6dso16is_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSO16IS_SPI) += st_lsm6dso16is_spi.o diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is.h b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is.h new file mode 100644 index 000000000000..3b91645708bb --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is.h @@ -0,0 +1,343 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dso16is sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSO16IS_H +#define ST_LSM6DSO16IS_H + +#include +#include +#include +#include +#include + +#define ST_LSM6DSO16IS_DEV_NAME "lsm6dso16is" + +#define ST_LSM6DSO16IS_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DSO16IS_SHUB_REG_MASK BIT(6) + +#define ST_LSM6DSO16IS_REG_PIN_CTRL_ADDR 0x02 +#define ST_LSM6DSO16IS_SDO_PU_EN_MASK BIT(6) + +#define ST_LSM6DSO16IS_REG_WHOAMI_ADDR 0x0f +#define ST_LSM6DSO16IS_WHOAMI_VAL 0x22 + +#define ST_LSM6DSO16IS_REG_CTRL1_XL_ADDR 0x10 +#define ST_LSM6DSO16IS_ODR_XL_MASK GENMASK(7, 4) +#define ST_LSM6DSO16IS_FS_XL_MASK GENMASK(3, 2) + +#define ST_LSM6DSO16IS_REG_CTRL2_G_ADDR 0x11 +#define ST_LSM6DSO16IS_ODR_G_MASK GENMASK(7, 4) +#define ST_LSM6DSO16IS_FS_G_MASK GENMASK(3, 1) + +#define ST_LSM6DSO16IS_REG_CTRL3_C_ADDR 0x12 +#define ST_LSM6DSO16IS_BOOT_MASK BIT(7) +#define ST_LSM6DSO16IS_BDU_MASK BIT(6) +#define ST_LSM6DSO16IS_H_LACTIVE_MASK BIT(5) +#define ST_LSM6DSO16IS_PP_OD_MASK BIT(4) +#define ST_LSM6DSO16IS_SW_RESET_MASK BIT(0) + +#define ST_LSM6DSO16IS_REG_CTRL5_C_ADDR 0x14 +#define ST_LSM6DSO16IS_ST_G_MASK GENMASK(3, 2) +#define ST_LSM6DSO16IS_ST_XL_MASK GENMASK(1, 0) + +#define ST_LSM6DSO16IS_REG_STATUS_ADDR 0x1e +#define ST_LSM6DSO16IS_STATUS_TDA BIT(2) +#define ST_LSM6DSO16IS_STATUS_XLDA BIT(0) +#define ST_LSM6DSO16IS_STATUS_GDA BIT(1) + +#define ST_LSM6DSO16IS_REG_OUT_TEMP_L_ADDR 0x20 +#define ST_LSM6DSO16IS_REG_OUTX_L_G_ADDR 0x22 +#define ST_LSM6DSO16IS_REG_OUTY_L_G_ADDR 0x24 +#define ST_LSM6DSO16IS_REG_OUTZ_L_G_ADDR 0x26 +#define ST_LSM6DSO16IS_REG_OUTX_L_A_ADDR 0x28 +#define ST_LSM6DSO16IS_REG_OUTY_L_A_ADDR 0x2a +#define ST_LSM6DSO16IS_REG_OUTZ_L_A_ADDR 0x2c + +#define ST_LSM6DSO16IS_ST_ACCEL_MIN 737 +#define ST_LSM6DSO16IS_ST_ACCEL_MAX 13934 +#define ST_LSM6DSO16IS_ST_GYRO_MIN 2142 +#define ST_LSM6DSO16IS_ST_GYRO_MAX 10000 + +#define ST_LSM6DSO16IS_ST_DISABLED_VAL 0 +#define ST_LSM6DSO16IS_ST_POS_SIGN_VAL 1 +#define ST_LSM6DSO16IS_ST_NEG_ACCEL_SIGN_VAL 2 +#define ST_LSM6DSO16IS_ST_NEG_GYRO_SIGN_VAL 3 + +/* shub registers */ +#define ST_LSM6DSO16IS_REG_SENSOR_HUB_1_ADDR 0x02 + +#define ST_LSM6DSO16IS_REG_MASTER_CONFIG_ADDR 0x14 +#define ST_LSM6DSO16IS_WRITE_ONCE_MASK BIT(6) +#define ST_LSM6DSO16IS_SHUB_PU_EN_MASK BIT(3) +#define ST_LSM6DSO16IS_MASTER_ON_MASK BIT(2) +#define ST_LSM6DSO16IS_AUX_SENS_ON_MASK GENMASK(1, 0) + +#define ST_LSM6DSO16IS_REG_SLV0_ADDR 0x15 +#define ST_LSM6DSO16IS_REG_SLV0_CFG 0x17 +#define ST_LSM6DSO16IS_REG_SLV1_ADDR 0x18 +#define ST_LSM6DSO16IS_REG_SLV2_ADDR 0x1b +#define ST_LSM6DSO16IS_REG_SLV3_ADDR 0x1e + +#define ST_LSM6DSO16IS_REG_DATAWRITE_SLV0_ADDR 0x21 +#define ST_LSM6DSO16IS_SLAVE_NUMOP_MASK GENMASK(2, 0) + +#define ST_LSM6DSO16IS_REG_STATUS_MASTER_ADDR 0x22 +#define ST_LSM6DSO16IS_SENS_HUB_ENDOP_MASK BIT(0) + +/* Timestamp Tick 25us/LSB */ +#define ST_LSM6DSO16IS_TS_DELTA_NS 25000ULL + +/* Temperature in uC */ +#define ST_LSM6DSO16IS_TEMP_GAIN 256 +#define ST_LSM6DSO16IS_TEMP_OFFSET 6400 + +#define ST_LSM6DSO16IS_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_LSM6DSO16IS_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +extern const struct dev_pm_ops st_lsm6dso16is_pm_ops; + +/** + * struct st_lsm6dso16is_reg - Generic sensor register + * description (addr + mask) + * + * @addr: Address of register. + * @mask: Bitmask register for proper usage. + */ +struct st_lsm6dso16is_reg { + u8 addr; + u8 mask; +}; + +/** + * struct st_lsm6dso16is_odr - Single ODR entry + * @mhz: Sensor ODR (milli Hz). + * @val: ODR register value. + */ +struct st_lsm6dso16is_odr { + u32 mhz; + u8 val; +}; + +/** + * struct st_lsm6dso16is_odr_table_entry - Sensor ODR table + * @size: Size of ODR table. + * @reg: ODR register. + * @odr_avl: Array of supported ODR value. + */ +struct st_lsm6dso16is_odr_table_entry { + u8 size; + struct st_lsm6dso16is_reg reg; + struct st_lsm6dso16is_odr odr_avl[8]; +}; + +/** + * struct st_lsm6dso16is_fs + * brief Full scale entry + * + * @gain: The gain to obtain data value from raw data (LSB). + * @val: Register value. + */ +struct st_lsm6dso16is_fs { + u32 gain; + u8 val; +}; + +/** + * struct st_lsm6dso16is_fs_table_entry - Full Scale sensor table + * @reg: st_lsm6dso16is_reg struct. + * @fs_avl: Full Scale list entries. + * @fs_len: Real size of fs_avl array. + */ +struct st_lsm6dso16is_fs_table_entry { + int fs_len; + struct st_lsm6dso16is_reg reg; + struct st_lsm6dso16is_fs fs_avl[4]; +}; + +enum st_lsm6dso16is_sensor_id { + ST_LSM6DSO16IS_ID_GYRO = 0, + ST_LSM6DSO16IS_ID_ACC, + ST_LSM6DSO16IS_ID_TEMP, + ST_LSM6DSO16IS_ID_EXT0, + ST_LSM6DSO16IS_ID_EXT1, + ST_LSM6DSO16IS_ID_MAX, +}; + +/** + * @enum st_lsm6dso_sensor_id + * @brief Sensor Table Identifier + */ +static const enum st_lsm6dso16is_sensor_id st_lsm6dso16is_main_sensor_list[] = { + [0] = ST_LSM6DSO16IS_ID_GYRO, + [1] = ST_LSM6DSO16IS_ID_ACC, + [2] = ST_LSM6DSO16IS_ID_TEMP, + [3] = ST_LSM6DSO16IS_ID_EXT0, + [4] = ST_LSM6DSO16IS_ID_EXT1, +}; + +static const enum st_lsm6dso16is_sensor_id +st_lsm6dso16is_triggered_main_sensor_list[] = { + [0] = ST_LSM6DSO16IS_ID_GYRO, + [1] = ST_LSM6DSO16IS_ID_ACC, + [2] = ST_LSM6DSO16IS_ID_TEMP, + [3] = ST_LSM6DSO16IS_ID_EXT0, + [4] = ST_LSM6DSO16IS_ID_EXT1, +}; + +struct st_lsm6dso16is_ext_dev_info { + const struct st_lsm6dso16is_ext_dev_settings *ext_dev_settings; + u8 ext_dev_i2c_addr; +}; + +/** + * struct st_lsm6dso16is_sensor - ST IMU sensor instance + * @ext_dev_info: For sensor hub indicate device info struct. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dso16is_hw. + * @name: Sensor name. + * @offset: Sensor data offset. + * @gain: Configured sensor sensitivity. + * @mhz: Output data rate of the sensor [milli Hz]. + * @selftest_status: Report last self test status. + * @min_st: Min self test raw data value. + * @max_st: Max self test raw data value. + */ +struct st_lsm6dso16is_sensor { + struct st_lsm6dso16is_ext_dev_info ext_dev_info; + enum st_lsm6dso16is_sensor_id id; + struct st_lsm6dso16is_hw *hw; + char name[32]; + + u32 offset; + u32 gain; + u32 mhz; + + /* self test */ + int8_t selftest_status; + int min_st; + int max_st; +}; + +/** + * struct st_lsm6dso16is_hw - ST IMU MEMS hw instance + * @iio_devs: Pointers to acc/gyro iio_dev instances. + * @orientation: Sensor orientation matrix. + * @vddio_supply: Voltage regulator for VDDIIO. + * @vdd_supply: Voltage regulator for VDD. + * @page_lock: Mutex to prevent concurrent access to the page selector. + * @regmap: Register map of the device. + * @dev: Pointer to instance of struct device (I2C or SPI). + * @i2c_master_pu: I2C master line Pull Up configuration. + * @enable_mask: Enabled sensor bitmask. + * @ext_data_len: Number of i2c slave devices connected to I2C master. + * @irq: Device interrupt line (I2C or SPI). + */ +struct st_lsm6dso16is_hw { + struct iio_dev *iio_devs[ST_LSM6DSO16IS_ID_MAX]; + struct iio_mount_matrix orientation; + struct regulator *vddio_supply; + struct regulator *vdd_supply; + struct mutex page_lock; + struct regmap *regmap; + struct device *dev; + u8 i2c_master_pu; + u32 enable_mask; + u8 ext_data_len; + int irq; +}; + +static inline int +__st_lsm6dso16is_write_with_mask(struct st_lsm6dso16is_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int val) +{ + return regmap_update_bits(hw->regmap, addr, mask, + ST_LSM6DSO16IS_SHIFT_VAL(val, mask)); +} + +static inline int +st_lsm6dso16is_update_bits_locked(struct st_lsm6dso16is_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dso16is_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dso16is_read_locked(struct st_lsm6dso16is_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dso16is_write_locked(struct st_lsm6dso16is_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_lsm6dso16is_set_page_access(struct st_lsm6dso16is_hw *hw, unsigned int mask, + unsigned int val) +{ + return __st_lsm6dso16is_write_with_mask(hw, + ST_LSM6DSO16IS_REG_FUNC_CFG_ACCESS_ADDR, + mask, val); +} + +int st_lsm6dso16is_probe(struct device *dev, int irq, struct regmap *regmap); +int st_lsm6dso16is_sensor_set_enable(struct st_lsm6dso16is_sensor *sensor, + bool enable); +int st_lsm6dso16is_shub_probe(struct st_lsm6dso16is_hw *hw); +int st_lsm6dso16is_shub_set_enable(struct st_lsm6dso16is_sensor *sensor, + bool enable); +int st_lsm6dso16is_shub_read(struct st_lsm6dso16is_sensor *sensor, + u8 addr, u8 *data, int len); +int st_lsm6dso16is_allocate_buffers(struct st_lsm6dso16is_hw *hw); +#endif /* ST_LSM6DSO16IS_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_core.c b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_core.c new file mode 100644 index 000000000000..0724ebe25b65 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_core.c @@ -0,0 +1,1210 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dso16is sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dso16is.h" + +static struct st_lsm6dso16is_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_lsm6dso16is_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DSO16IS_ST_DISABLED_VAL, + .gyro_value = ST_LSM6DSO16IS_ST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DSO16IS_ST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DSO16IS_ST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DSO16IS_ST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DSO16IS_ST_NEG_GYRO_SIGN_VAL + }, +}; + +static const struct st_lsm6dso16is_odr_table_entry st_lsm6dso16is_odr_table[] = { + [ST_LSM6DSO16IS_ID_ACC] = { + .size = 7, + .reg = { + .addr = ST_LSM6DSO16IS_REG_CTRL1_XL_ADDR, + .mask = ST_LSM6DSO16IS_ODR_XL_MASK, + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_avl[6] = { 833000, 0x07 }, + }, + [ST_LSM6DSO16IS_ID_GYRO] = { + .size = 7, + .reg = { + .addr = ST_LSM6DSO16IS_REG_CTRL2_G_ADDR, + .mask = ST_LSM6DSO16IS_ODR_G_MASK, + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_avl[6] = { 833000, 0x07 }, + }, + [ST_LSM6DSO16IS_ID_TEMP] = { + .size = 2, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 52000, 0x03 }, + }, +}; + +static struct st_lsm6dso16is_fs_table_entry st_lsm6dso16is_fs_table[] = { + [ST_LSM6DSO16IS_ID_ACC] = { + .fs_len = 4, + .reg = { + .addr = ST_LSM6DSO16IS_REG_CTRL1_XL_ADDR, + .mask = ST_LSM6DSO16IS_FS_XL_MASK, + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + }, + [ST_LSM6DSO16IS_ID_GYRO] = { + .fs_len = 4, + .reg = { + .addr = ST_LSM6DSO16IS_REG_CTRL2_G_ADDR, + .mask = ST_LSM6DSO16IS_FS_G_MASK, + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + }, + [ST_LSM6DSO16IS_ID_TEMP] = { + .fs_len = 1, + .fs_avl[0] = { ST_LSM6DSO16IS_TEMP_GAIN, 0x0 }, + }, +}; + +static const struct iio_mount_matrix * +st_lsm6dso16is_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *ch) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dso16is_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct iio_chan_spec_ext_info st_lsm6dso16is_chan_spec_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, st_lsm6dso16is_get_mount_matrix), + { } +}; + +static const struct iio_chan_spec st_lsm6dso16is_acc_channels[] = { + ST_LSM6DSO16IS_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSO16IS_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dso16is_chan_spec_ext_info), + ST_LSM6DSO16IS_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSO16IS_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dso16is_chan_spec_ext_info), + ST_LSM6DSO16IS_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSO16IS_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dso16is_chan_spec_ext_info), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lsm6dso16is_gyro_channels[] = { + ST_LSM6DSO16IS_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSO16IS_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dso16is_chan_spec_ext_info), + ST_LSM6DSO16IS_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSO16IS_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dso16is_chan_spec_ext_info), + ST_LSM6DSO16IS_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSO16IS_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dso16is_chan_spec_ext_info), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lsm6dso16is_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LSM6DSO16IS_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, + } + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static __maybe_unused int st_lsm6dso16is_reg_access(struct iio_dev *iio_dev, + unsigned int reg, + unsigned int writeval, + unsigned int *readval) +{ + struct st_lsm6dso16is_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; +} + +/** + * st_lsm6dso16is_check_whoami - Detect device HW ID + * + * Check the value of the device HW ID if valid + * + * @param hw: ST IMU MEMS hw instance. + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dso16is_check_whoami(struct st_lsm6dso16is_hw *hw) +{ + int err, data; + + err = regmap_read(hw->regmap, ST_LSM6DSO16IS_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + + return err; + } + + if (data != ST_LSM6DSO16IS_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + + return -ENODEV; + } + + return 0; +} + +/** + * st_lsm6dso16is_set_full_scale - Set sensor full scale + * + * Check the value of requested gain, apply it if supported. + * NOTE: support also sensor with only one FS available (Temp gain is fixed). + * + * @param sensor: ST IMU MEMS sensor instance. + * @param gain: Sensor gain. + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dso16is_set_full_scale(struct st_lsm6dso16is_sensor *sensor, + u32 gain) +{ + enum st_lsm6dso16is_sensor_id id = sensor->id; + struct st_lsm6dso16is_hw *hw = sensor->hw; + int i, err; + u8 val; + + for (i = 0; i < st_lsm6dso16is_fs_table[id].fs_len; i++) + if (st_lsm6dso16is_fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == st_lsm6dso16is_fs_table[id].fs_len) + return -EINVAL; + + val = st_lsm6dso16is_fs_table[id].fs_avl[i].val; + err = regmap_update_bits(hw->regmap, + st_lsm6dso16is_fs_table[id].reg.addr, + st_lsm6dso16is_fs_table[id].reg.mask, + ST_LSM6DSO16IS_SHIFT_VAL(val, + st_lsm6dso16is_fs_table[id].reg.mask)); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dso16is_get_odr_val(enum st_lsm6dso16is_sensor_id id, u32 mhz, + u32 *val, u32 *modr) +{ + u32 sensor_odr; + int i; + + /* this avoid entry 0mHz to ODR table */ + if (mhz == 0) { + *val = 0; + *modr = 0; + + return 0; + } + + for (i = 0; i < st_lsm6dso16is_odr_table[id].size; i++) { + sensor_odr = st_lsm6dso16is_odr_table[id].odr_avl[i].mhz; + if (sensor_odr >= mhz) { + *val = st_lsm6dso16is_odr_table[id].odr_avl[i].val; + *modr = sensor_odr; + + return 0; + } + } + + return -EINVAL; +} + + +static u32 +st_lsm6dso16is_check_odr_dependency(struct st_lsm6dso16is_hw *hw, u32 mhz, + enum st_lsm6dso16is_sensor_id ref_id) +{ + struct st_lsm6dso16is_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + u32 ret = mhz; + + if (mhz > 0) { + if (hw->enable_mask & BIT(ref_id)) + ret = max_t(u32, ref->mhz, mhz); + } else { + ret = (hw->enable_mask & BIT(ref_id)) ? ref->mhz : 0; + } + + return ret; +} + +static int st_lsm6dso16is_set_odr(struct st_lsm6dso16is_sensor *sensor, u32 mhz) +{ + enum st_lsm6dso16is_sensor_id id = sensor->id; + struct st_lsm6dso16is_hw *hw = sensor->hw; + int val, err, odr, modr; + + switch (id) { + case ST_LSM6DSO16IS_ID_EXT0: + case ST_LSM6DSO16IS_ID_EXT1: + case ST_LSM6DSO16IS_ID_TEMP: + case ST_LSM6DSO16IS_ID_ACC: { + int i; + + id = ST_LSM6DSO16IS_ID_ACC; + for (i = ST_LSM6DSO16IS_ID_ACC; i < ST_LSM6DSO16IS_ID_MAX; i++) { + if (!hw->iio_devs[i] || i == sensor->id) + continue; + + odr = st_lsm6dso16is_check_odr_dependency(hw, mhz, i); + if (odr != mhz) + return 0; + } + break; + } + default: + break; + } + + err = st_lsm6dso16is_get_odr_val(id, odr, &val, &modr); + if (err < 0) + return err; + + return st_lsm6dso16is_update_bits_locked(hw, + st_lsm6dso16is_odr_table[id].reg.addr, + st_lsm6dso16is_odr_table[id].reg.mask, + val); +} + +int st_lsm6dso16is_sensor_set_enable(struct st_lsm6dso16is_sensor *sensor, + bool enable) +{ + int mhz = enable ? sensor->mhz : 0; + int err; + + err = st_lsm6dso16is_set_odr(sensor, mhz); + 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 int st_lsm6dso16is_read_oneshot(struct st_lsm6dso16is_sensor *sensor, + u8 addr, int *val) +{ + struct st_lsm6dso16is_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + /* + * adjust delay for data valid because of turn-on time: + * - Acc, Power-down -> High-performance discard 1 sample + * - Gyro, Power-down -> High-performance wait 70 ms + f(ODR) + * - Temp, 1 ODR + * NOTE: we use conversion 1100000000 to also take into account the + * internal oscillator tolerance of 10% + */ + switch (sensor->id) { + case ST_LSM6DSO16IS_ID_GYRO: { + int n = 3; + + if (sensor->mhz > + st_lsm6dso16is_odr_table[sensor->id].odr_avl[0].mhz) + n++; + + delay = 70000 + n * (1100000000 / sensor->mhz); + } + break; + case ST_LSM6DSO16IS_ID_ACC: + delay = 2 * (1100000000 / sensor->mhz); + break; + case ST_LSM6DSO16IS_ID_TEMP: + delay = 1100000000 / sensor->mhz; + break; + default: + return -EINVAL; + } + + err = st_lsm6dso16is_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + usleep_range(delay, delay + (delay >> 1)); + err = st_lsm6dso16is_read_locked(hw, addr, &data, sizeof(data)); + st_lsm6dso16is_sensor_set_enable(sensor, false); + if (err < 0) + return err; + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dso16is_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 -EINVAL; + } + default: + return IIO_VAL_INT_PLUS_MICRO; + } + + return -EINVAL; +} + +static int st_lsm6dso16is_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dso16is_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_lsm6dso16is_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->mhz / 1000; + *val2 = (sensor->mhz % 1000) * 1000; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SCALE: + switch (ch->type) { + case IIO_TEMP: + *val = 1000; + *val2 = ST_LSM6DSO16IS_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; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lsm6dso16is_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dso16is_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + u32 reg, modr; + + val = val * 1000 + val2 / 1000; + + err = st_lsm6dso16is_get_odr_val(sensor->id, val, ®, &modr); + if (err < 0) + goto release; + + sensor->mhz = modr; + break; + } + default: + err = -EINVAL; + break; + } + +release: + iio_device_release_direct_mode(iio_dev); + + return err; +} + +static ssize_t +st_lsm6dso16is_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + const struct st_lsm6dso16is_odr_table_entry *odr_table; + enum st_lsm6dso16is_sensor_id id = sensor->id; + int i, len = 0; + + odr_table = &st_lsm6dso16is_odr_table[id]; + for (i = 0; i < st_lsm6dso16is_odr_table[id].size; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ", + odr_table->odr_avl[i].mhz / 1000, + odr_table->odr_avl[i].mhz % 1000); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dso16is_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dso16is_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dso16is_fs_table[id].fs_len; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_lsm6dso16is_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static int +st_lsm6dso16is_set_selftest(struct st_lsm6dso16is_sensor *sensor, int index) +{ + u8 mode, mask; + + switch (sensor->id) { + case ST_LSM6DSO16IS_ID_ACC: + mask = ST_LSM6DSO16IS_ST_XL_MASK; + mode = st_lsm6dso16is_selftest_table[index].accel_value; + break; + case ST_LSM6DSO16IS_ID_GYRO: + mask = ST_LSM6DSO16IS_ST_G_MASK; + mode = st_lsm6dso16is_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_lsm6dso16is_update_bits_locked(sensor->hw, + ST_LSM6DSO16IS_REG_CTRL5_C_ADDR, + mask, mode); +} + +static ssize_t +st_lsm6dso16is_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6dso16is_selftest_table[1].string_mode, + st_lsm6dso16is_selftest_table[2].string_mode); +} + +static ssize_t +st_lsm6dso16is_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dso16is_sensor_id id = sensor->id; + char *message = NULL; + int8_t result; + + if (id != ST_LSM6DSO16IS_ID_ACC && + id != ST_LSM6DSO16IS_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_lsm6dso16is_selftest_sensor(struct st_lsm6dso16is_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_LSM6DSO16IS_ID_ACC: + reg = ST_LSM6DSO16IS_REG_OUTX_L_A_ADDR; + bitmask = ST_LSM6DSO16IS_STATUS_XLDA; + break; + case ST_LSM6DSO16IS_ID_GYRO: + reg = ST_LSM6DSO16IS_REG_OUTX_L_G_ADDR; + bitmask = ST_LSM6DSO16IS_STATUS_GDA; + break; + default: + return -EINVAL; + } + + /* set selftest normal mode */ + ret = st_lsm6dso16is_set_selftest(sensor, 0); + if (ret < 0) + return ret; + + ret = st_lsm6dso16is_sensor_set_enable(sensor, true); + if (ret < 0) + return ret; + + /* wait at least 2 ODRs to be sure */ + delay = 2 * (1000000000 / sensor->mhz); + + /* 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, 2 * delay); + ret = st_lsm6dso16is_read_locked(sensor->hw, + ST_LSM6DSO16IS_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dso16is_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 acc samples missing (expected %d, read %d)\n", + i, n); + ret = -1; + + goto selftest_failure; + } + + n = 0; + + /* set selftest mode */ + st_lsm6dso16is_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, 2 * delay); + ret = st_lsm6dso16is_read_locked(sensor->hw, + ST_LSM6DSO16IS_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dso16is_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_lsm6dso16is_set_selftest(sensor, 0); + + return st_lsm6dso16is_sensor_set_enable(sensor, false); +} + +static ssize_t +st_lsm6dso16is_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_lsm6dso16is_sensor *sensor = iio_priv(iio_dev); + enum st_lsm6dso16is_sensor_id id = sensor->id; + struct st_lsm6dso16is_hw *hw = sensor->hw; + int ret, test; + u32 gain, mhz; + + if (id != ST_LSM6DSO16IS_ID_ACC && + id != ST_LSM6DSO16IS_ID_GYRO) + return -EINVAL; + + for (test = 0; test < ARRAY_SIZE(st_lsm6dso16is_selftest_table); test++) { + if (strncmp(buf, st_lsm6dso16is_selftest_table[test].string_mode, + strlen(st_lsm6dso16is_selftest_table[test].string_mode)) == 0) + break; + } + + if (test == ARRAY_SIZE(st_lsm6dso16is_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; + } + + gain = sensor->gain; + mhz = sensor->mhz; + if (id == ST_LSM6DSO16IS_ID_ACC) { + /* set BDU = 1, FS = 4 g, ODR = 52 Hz */ + st_lsm6dso16is_set_full_scale(sensor, IIO_G_TO_M_S_2(122)); + st_lsm6dso16is_set_odr(sensor, 52000); + st_lsm6dso16is_selftest_sensor(sensor, test); + } else { + /* set BDU = 1, ODR = 208 Hz, FS = 2000 dps */ + st_lsm6dso16is_set_full_scale(sensor, IIO_DEGREE_TO_RAD(70000)); + st_lsm6dso16is_set_odr(sensor, 208000); + st_lsm6dso16is_selftest_sensor(sensor, test); + } + + /* restore full scale after test */ + st_lsm6dso16is_set_full_scale(sensor, gain); + st_lsm6dso16is_set_odr(sensor, mhz); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dso16is_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dso16is_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dso16is_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_lsm6dso16is_sysfs_scale_avail, NULL, 0); + +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lsm6dso16is_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_lsm6dso16is_sysfs_get_selftest_status, + st_lsm6dso16is_sysfs_start_selftest, 0); + +static struct attribute *st_lsm6dso16is_acc_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dso16is_acc_attribute_group = { + .attrs = st_lsm6dso16is_acc_attributes, +}; + +static const struct iio_info st_lsm6dso16is_acc_info = { + .attrs = &st_lsm6dso16is_acc_attribute_group, + .read_raw = st_lsm6dso16is_read_raw, + .write_raw = st_lsm6dso16is_write_raw, + .write_raw_get_fmt = st_lsm6dso16is_write_raw_get_fmt, + .debugfs_reg_access = st_lsm6dso16is_reg_access, +}; + +static struct attribute *st_lsm6dso16is_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_selftest_available.dev_attr.attr, + &iio_dev_attr_selftest.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dso16is_gyro_attribute_group = { + .attrs = st_lsm6dso16is_gyro_attributes, +}; + +static const struct iio_info st_lsm6dso16is_gyro_info = { + .attrs = &st_lsm6dso16is_gyro_attribute_group, + .read_raw = st_lsm6dso16is_read_raw, + .write_raw = st_lsm6dso16is_write_raw, + .write_raw_get_fmt = st_lsm6dso16is_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dso16is_temp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_temp_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dso16is_temp_attribute_group = { + .attrs = st_lsm6dso16is_temp_attributes, +}; + +static const struct iio_info st_lsm6dso16is_temp_info = { + .attrs = &st_lsm6dso16is_temp_attribute_group, + .read_raw = st_lsm6dso16is_read_raw, + .write_raw = st_lsm6dso16is_write_raw, + .write_raw_get_fmt = st_lsm6dso16is_write_raw_get_fmt, +}; + +static int st_lsm6dso16is_reset_device(struct st_lsm6dso16is_hw *hw) +{ + int err; + + /* sw reset */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSO16IS_REG_CTRL3_C_ADDR, + ST_LSM6DSO16IS_SW_RESET_MASK, + FIELD_PREP(ST_LSM6DSO16IS_SW_RESET_MASK, 1)); + if (err < 0) + return err; + + /* software reset procedure takes a maximum of 50 µs */ + usleep_range(50, 60); + + return err; +} + +static int st_lsm6dso16is_init_device(struct st_lsm6dso16is_hw *hw) +{ + /* enable Block Data Update */ + return regmap_update_bits(hw->regmap, ST_LSM6DSO16IS_REG_CTRL3_C_ADDR, + ST_LSM6DSO16IS_BDU_MASK, + FIELD_PREP(ST_LSM6DSO16IS_BDU_MASK, 1)); +} + +static struct iio_dev *st_lsm6dso16is_alloc_iiodev(struct st_lsm6dso16is_hw *hw, + enum st_lsm6dso16is_sensor_id id) +{ + struct st_lsm6dso16is_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; + + switch (id) { + case ST_LSM6DSO16IS_ID_ACC: + iio_dev->channels = st_lsm6dso16is_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dso16is_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), "%s_accel", + ST_LSM6DSO16IS_DEV_NAME); + iio_dev->info = &st_lsm6dso16is_acc_info; + st_lsm6dso16is_set_full_scale(sensor, + st_lsm6dso16is_fs_table[id].fs_avl[0].gain); + sensor->offset = 0; + sensor->mhz = st_lsm6dso16is_odr_table[id].odr_avl[1].mhz; + sensor->min_st = ST_LSM6DSO16IS_ST_ACCEL_MIN; + sensor->max_st = ST_LSM6DSO16IS_ST_ACCEL_MAX; + break; + case ST_LSM6DSO16IS_ID_GYRO: + iio_dev->channels = st_lsm6dso16is_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dso16is_gyro_channels); + scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro", + ST_LSM6DSO16IS_DEV_NAME); + iio_dev->info = &st_lsm6dso16is_gyro_info; + st_lsm6dso16is_set_full_scale(sensor, + st_lsm6dso16is_fs_table[id].fs_avl[0].gain); + sensor->offset = 0; + sensor->mhz = st_lsm6dso16is_odr_table[id].odr_avl[1].mhz; + sensor->min_st = ST_LSM6DSO16IS_ST_GYRO_MIN; + sensor->max_st = ST_LSM6DSO16IS_ST_GYRO_MAX; + break; + case ST_LSM6DSO16IS_ID_TEMP: + iio_dev->channels = st_lsm6dso16is_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dso16is_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), "%s_temp", + ST_LSM6DSO16IS_DEV_NAME); + iio_dev->info = &st_lsm6dso16is_temp_info; + sensor->offset = ST_LSM6DSO16IS_TEMP_OFFSET; + sensor->mhz = st_lsm6dso16is_odr_table[id].odr_avl[1].mhz; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static void st_lsm6dso16is_disable_regulator_action(void *_data) +{ + struct st_lsm6dso16is_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static int st_lsm6dso16is_power_enable(struct st_lsm6dso16is_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_lsm6dso16is_disable_regulator_action, + hw); + if (err) { + dev_err(hw->dev, + "Failed to setup regulator cleanup action %d\n", + err); + return err; + } + + return 0; +} + +/** + * Probe device function + * Implements [MODULE] feature for Power Management + * + * @param dev: Device pointer. + * @param irq: I2C/SPI/I3C client irq. + * @param hw_id: Sensor HW id. + * @param regmap: Bus Transfer Function pointer. + * @retval 0 if OK, < 0 for error + */ +int st_lsm6dso16is_probe(struct device *dev, int irq, struct regmap *regmap) +{ + struct st_lsm6dso16is_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->page_lock); + + hw->regmap = regmap; + hw->dev = dev; + hw->irq = irq; + + err = st_lsm6dso16is_power_enable(hw); + if (err != 0) + return err; + + /* + * After the device is powered up, it performs a 10 ms (maximum) boot + * procedure to load the trimming parameters. + * After the boot is completed, both the accelerometer and the gyroscope + * are automatically configured in power-down mode. + */ + usleep_range(10000, 11000); + + err = regmap_write(hw->regmap, + ST_LSM6DSO16IS_REG_FUNC_CFG_ACCESS_ADDR, 0); + if (err < 0) + return err; + + err = st_lsm6dso16is_check_whoami(hw); + if (err < 0) + return err; + + err = st_lsm6dso16is_reset_device(hw); + if (err < 0) + return err; + + err = st_lsm6dso16is_init_device(hw); + if (err < 0) + return err; + +#if KERNEL_VERSION(5, 15, 0) <= LINUX_VERSION_CODE + err = iio_read_mount_matrix(hw->dev, &hw->orientation); +#elif KERNEL_VERSION(5, 2, 0) <= LINUX_VERSION_CODE + err = iio_read_mount_matrix(hw->dev, "mount-matrix", &hw->orientation); +#else /* LINUX_VERSION_CODE */ + err = of_iio_read_mount_matrix(hw->dev, "mount-matrix", + &hw->orientation); +#endif /* LINUX_VERSION_CODE */ + + if (err) { + dev_err(dev, "Failed to retrieve mounting matrix %d\n", err); + + return err; + } + + /* register only data sensors */ + for (i = 0; i < ARRAY_SIZE(st_lsm6dso16is_main_sensor_list); i++) { + enum st_lsm6dso16is_sensor_id id = st_lsm6dso16is_main_sensor_list[i]; + + hw->iio_devs[id] = st_lsm6dso16is_alloc_iiodev(hw, id); + if (!hw->iio_devs[id]) + continue; + } + + if (!dev_fwnode(dev) || + device_property_read_bool(dev, "enable-sensor-hub")) { + err = st_lsm6dso16is_shub_probe(hw); + if (err < 0) + return err; + } + + err = st_lsm6dso16is_allocate_buffers(hw); + if (err < 0) + return err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dso16is_main_sensor_list); i++) { + enum st_lsm6dso16is_sensor_id id = st_lsm6dso16is_main_sensor_list[i]; + + if (!hw->iio_devs[id]) + continue; + + err = devm_iio_device_register(hw->dev, hw->iio_devs[id]); + if (err) + return err; + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6dso16is_probe); + +static int __maybe_unused st_lsm6dso16is_suspend(struct device *dev) +{ + struct st_lsm6dso16is_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dso16is_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSO16IS_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_lsm6dso16is_set_odr(sensor, 0); + if (err < 0) + return err; + } + + return err < 0 ? err : 0; +} + +static int __maybe_unused st_lsm6dso16is_resume(struct device *dev) +{ + struct st_lsm6dso16is_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dso16is_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSO16IS_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_lsm6dso16is_set_odr(sensor, sensor->mhz); + if (err < 0) + return err; + } + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_lsm6dso16is_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dso16is_suspend, st_lsm6dso16is_resume) +}; +EXPORT_SYMBOL(st_lsm6dso16is_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dso16is driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_i2c.c b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_i2c.c new file mode 100644 index 000000000000..f615db099fe3 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_i2c.c @@ -0,0 +1,65 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dso16is i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dso16is.h" + +static const struct regmap_config st_lsm6dso16is_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dso16is_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, + &st_lsm6dso16is_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_lsm6dso16is_probe(&client->dev, client->irq, regmap); +} + +static const struct of_device_id st_lsm6dso16is_i2c_of_match[] = { + { .compatible = "st,lsm6dso16is", }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dso16is_i2c_of_match); + +static const struct i2c_device_id st_lsm6dso16is_i2c_id_table[] = { + { ST_LSM6DSO16IS_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dso16is_i2c_id_table); + +static struct i2c_driver st_lsm6dso16is_driver = { + .driver = { + .name = "st_" ST_LSM6DSO16IS_DEV_NAME "_i2c", + .pm = &st_lsm6dso16is_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dso16is_i2c_of_match), + }, + .probe = st_lsm6dso16is_i2c_probe, + .id_table = st_lsm6dso16is_i2c_id_table, +}; +module_i2c_driver(st_lsm6dso16is_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dso16is i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_shub.c b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_shub.c new file mode 100644 index 000000000000..67e8af86d9c2 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_shub.c @@ -0,0 +1,961 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dso16is sensor hub library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dso16is.h" + +#define ST_LSM6DSO16IS_MAX_SLV_NUM 2 + +/** + * @struct st_lsm6dso16is_ext_pwr + * @brief External device Power Management description + * reg: Generic sensor register description. + * off_val: Value to write into register to power off external sensor. + * on_val: Value to write into register for power on external sensor. + */ +struct st_lsm6dso16is_ext_pwr { + struct st_lsm6dso16is_reg reg; + u8 off_val; + u8 on_val; +}; + +/** + * @struct st_lsm6dso16is_ext_dev_settings + * @brief External sensor descritor entry + * i2c_addr: External I2C device address (max two). + * wai_addr: Device ID address. + * wai_val: Device ID value. + * odr_table: ODR sensor table. + * fs_table: Full scale table. + * temp_comp_reg: Temperature compensation registers. + * pwr_table: External device Power Management description. + * off_canc_reg: Offset cancellation registers. + * bdu_reg: Block Data Update registers. + * ext_channels:IIO device channel specifications. + * ext_chan_depth: Max number of IIO device channel specifications. + * data_len: Sensor output data len. + */ +struct st_lsm6dso16is_ext_dev_settings { + u8 i2c_addr[2]; + u8 wai_addr; + u8 wai_val; + struct st_lsm6dso16is_odr_table_entry odr_table; + struct st_lsm6dso16is_fs_table_entry fs_table; + struct st_lsm6dso16is_reg temp_comp_reg; + struct st_lsm6dso16is_ext_pwr pwr_table; + struct st_lsm6dso16is_reg off_canc_reg; + struct st_lsm6dso16is_reg bdu_reg; + const struct iio_chan_spec ext_channels[5]; + u8 ext_chan_depth; + u8 data_len; +}; + +static const struct st_lsm6dso16is_ext_dev_settings st_lsm6dso16is_ext_dev_table[] = { + { + /* LIS2MDL */ + .i2c_addr = { 0x1e }, + .wai_addr = 0x4f, + .wai_val = 0x40, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + .odr_avl[0] = { 5000, 0x0 }, + .odr_avl[1] = { 10000, 0x0 }, + .odr_avl[2] = { 20000, 0x1 }, + .odr_avl[3] = { 50000, 0x2 }, + .odr_avl[4] = { 100000, 0x3 }, + }, + .fs_table = { + .fs_len = 1, + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + }, + .temp_comp_reg = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc_reg = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu_reg = { + .addr = 0x62, + .mask = BIT(4), + }, + .ext_channels[0] = ST_LSM6DSO16IS_DATA_CHANNEL(IIO_MAGN, 0x68, + 1, IIO_MOD_X, 0, + 16, 16, 's', NULL), + .ext_channels[1] = ST_LSM6DSO16IS_DATA_CHANNEL(IIO_MAGN, 0x6a, + 1, IIO_MOD_Y, 1, + 16, 16, 's', NULL), + .ext_channels[2] = ST_LSM6DSO16IS_DATA_CHANNEL(IIO_MAGN, 0x6c, + 1, IIO_MOD_Z, 2, + 16, 16, 's', NULL), + .ext_channels[3] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 4, + .data_len = 6, + }, + { + /* LPS22HH */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb3, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1000, 0x1 }, + .odr_avl[1] = { 10000, 0x2 }, + .odr_avl[2] = { 25000, 0x3 }, + .odr_avl[3] = { 50000, 0x4 }, + .odr_avl[4] = { 100000, 0x6 }, + }, + .fs_table = { + .fs_len = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL / 4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_channels[0] = ST_LSM6DSO16IS_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u', NULL), + .ext_channels[1] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 2, + .data_len = 3, + }, +}; + +/** + * Wait write trigger [SHUB] + * + * In write on external device register, each operation is triggered + * by accel/gyro data ready, this means that wait time depends on ODR + * plus i2c time + * NOTE: Be sure to enable Acc or Gyro before this operation + * + * @param hw: ST IMU MEMS hw instance. + */ +static inline void st_lsm6dso16is_shub_wait_complete(struct st_lsm6dso16is_hw *hw) +{ + struct st_lsm6dso16is_sensor *sensor; + u32 odr; + + sensor = iio_priv(hw->iio_devs[ST_LSM6DSO16IS_ID_ACC]); + + /* check if acc is enabled (it should be) */ + if (hw->enable_mask & BIT(ST_LSM6DSO16IS_ID_ACC)) + odr = sensor->mhz; + else + odr = 12500; + + /* odr tollerance is 10 % */ + msleep(1100000 / odr); +} + +/** + * Read from sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dso16is_shub_read_reg(struct st_lsm6dso16is_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dso16is_set_page_access(hw, ST_LSM6DSO16IS_SHUB_REG_MASK, + true); + if (err < 0) + goto out; + + err = regmap_bulk_read(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dso16is_set_page_access(hw, ST_LSM6DSO16IS_SHUB_REG_MASK, + false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Write to sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dso16is_shub_write_reg(struct st_lsm6dso16is_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dso16is_set_page_access(hw, ST_LSM6DSO16IS_SHUB_REG_MASK, + true); + if (err < 0) + goto out; + + err = regmap_bulk_write(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dso16is_set_page_access(hw, ST_LSM6DSO16IS_SHUB_REG_MASK, + false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable sensor hub interface [SHUB] + * + * NOTE: uses page_lock + * + * @param sensor: ST IMU sensor instance + * @param enable: Master Enable/Disable. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_lsm6dso16is_shub_master_enable(struct st_lsm6dso16is_sensor *sensor, + bool enable) +{ + struct st_lsm6dso16is_hw *hw = sensor->hw; + int err; + + /* enable main sensor as trigger */ + err = st_lsm6dso16is_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dso16is_set_page_access(hw, ST_LSM6DSO16IS_SHUB_REG_MASK, + true); + if (err < 0) + goto out; + + err = __st_lsm6dso16is_write_with_mask(hw, + ST_LSM6DSO16IS_REG_MASTER_CONFIG_ADDR, + ST_LSM6DSO16IS_MASTER_ON_MASK, + enable); + + st_lsm6dso16is_set_page_access(hw, ST_LSM6DSO16IS_SHUB_REG_MASK, + false); + +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Read sensor data register from shub interface + * + * NOTE: use SLV3 i2c slave for one-shot read operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +int st_lsm6dso16is_shub_read(struct st_lsm6dso16is_sensor *sensor, + u8 addr, u8 *data, int len) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dso16is_hw *hw = sensor->hw; + u8 out_addr = ST_LSM6DSO16IS_REG_SENSOR_HUB_1_ADDR + hw->ext_data_len; + u8 config[3]; + int err; + + config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[1] = addr; + config[2] = len & 0x7; + + err = st_lsm6dso16is_shub_write_reg(hw, ST_LSM6DSO16IS_REG_SLV3_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dso16is_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dso16is_shub_wait_complete(hw); + + err = st_lsm6dso16is_shub_read_reg(hw, out_addr, data, len & 0x7); + + st_lsm6dso16is_shub_master_enable(sensor, false); + + memset(config, 0, sizeof(config)); + + return st_lsm6dso16is_shub_write_reg(hw, ST_LSM6DSO16IS_REG_SLV3_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface + * + * NOTE: use SLV0 i2c slave for write operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dso16is_shub_write(struct st_lsm6dso16is_sensor *sensor, + u8 addr, u8 *data, int len) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dso16is_hw *hw = sensor->hw; + u8 mconfig = ST_LSM6DSO16IS_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + u8 config[3] = {}; + int err, i; + + /* AuxSens = 3 + wr once + pull up configuration */ + err = st_lsm6dso16is_shub_write_reg(hw, + ST_LSM6DSO16IS_REG_MASTER_CONFIG_ADDR, + &mconfig, sizeof(mconfig)); + if (err < 0) + return err; + + config[0] = ext_info->ext_dev_i2c_addr << 1; + for (i = 0; i < len; i++) { + config[1] = addr + i; + + err = st_lsm6dso16is_shub_write_reg(hw, + ST_LSM6DSO16IS_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dso16is_shub_write_reg(hw, + ST_LSM6DSO16IS_REG_DATAWRITE_SLV0_ADDR, + &data[i], 1); + if (err < 0) + return err; + + err = st_lsm6dso16is_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dso16is_shub_wait_complete(hw); + + st_lsm6dso16is_shub_master_enable(sensor, false); + } + + return st_lsm6dso16is_shub_write_reg(hw, ST_LSM6DSO16IS_REG_SLV0_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface using register bitmask + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param mask: Register bitmask. + * @param val: Data buffer. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_lsm6dso16is_shub_write_with_mask(struct st_lsm6dso16is_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_lsm6dso16is_shub_read(sensor, addr, &data, sizeof(data)); + if (err < 0) + return err; + + data = (data & ~mask) | ST_LSM6DSO16IS_SHIFT_VAL(val, mask); + + return st_lsm6dso16is_shub_write(sensor, addr, &data, sizeof(data)); +} + +/** + * Configure external sensor connected on master I2C interface + * + * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_lsm6dso16is_shub_config_channels(struct st_lsm6dso16is_sensor *sensor, + bool enable) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info; + struct st_lsm6dso16is_hw *hw = sensor->hw; + struct st_lsm6dso16is_sensor *cur_sensor; + u8 config[6] = {}, enable_mask; + int i, j = 0; + + enable_mask = enable ? hw->enable_mask | BIT(sensor->id) + : hw->enable_mask & ~BIT(sensor->id); + + for (i = ST_LSM6DSO16IS_ID_EXT0; i <= ST_LSM6DSO16IS_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT(cur_sensor->id))) + continue; + + ext_info = &cur_sensor->ext_dev_info; + config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[j + 1] = + ext_info->ext_dev_settings->ext_channels[0].address; + config[j + 2] = (ext_info->ext_dev_settings->data_len & + ST_LSM6DSO16IS_SLAVE_NUMOP_MASK); + j += 3; + } + + return st_lsm6dso16is_shub_write_reg(hw, ST_LSM6DSO16IS_REG_SLV1_ADDR, + config, sizeof(config)); +} + +/** + * Get a valid ODR [SHUB] + * + * Check a valid ODR closest to the passed value + * + * @param sensor: SST IMU sensor instance. + * @param odr: ODR value (in Hz). + * @param val: ODR register value data pointer. + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dso16is_shub_get_odr_val(struct st_lsm6dso16is_sensor *sensor, + u32 odr, u8 *val) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) + if (ext_info->ext_dev_settings->odr_table.odr_avl[i].mhz >= odr) + break; + + if (i == ext_info->ext_dev_settings->odr_table.size) + return -EINVAL; + + *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val; + + return 0; +} + +/** + * Set new ODR to sensor [SHUB] + * + * Set a valid ODR closest to the passed value + * + * @param sensor: ST IMU sensor instance + * @param odr: ODR value (in Hz). + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dso16is_shub_set_odr(struct st_lsm6dso16is_sensor *sensor, + u32 odr) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dso16is_hw *hw = sensor->hw; + u8 odr_val; + int err; + + err = st_lsm6dso16is_shub_get_odr_val(sensor, odr, &odr_val); + if (err < 0) + return err; + + if (sensor->mhz == odr && (hw->enable_mask & BIT(sensor->id))) + return 0; + + return st_lsm6dso16is_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + odr_val); +} + +/** + * Enable or Disable sensor [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable or disable the sensor [true,false]. + * @return 0 if OK, negative value for ERROR + */ +int st_lsm6dso16is_shub_set_enable(struct st_lsm6dso16is_sensor *sensor, + bool enable) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err; + + err = st_lsm6dso16is_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + if (enable) { + err = st_lsm6dso16is_shub_set_odr(sensor, sensor->mhz); + if (err < 0) + return err; + } else { + err = st_lsm6dso16is_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + 0); + if (err < 0) + return err; + } + + if (ext_info->ext_dev_settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? ext_info->ext_dev_settings->pwr_table.on_val + : ext_info->ext_dev_settings->pwr_table.off_val; + err = st_lsm6dso16is_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->pwr_table.reg.addr, + ext_info->ext_dev_settings->pwr_table.reg.mask, + val); + if (err < 0) + return err; + } + + return st_lsm6dso16is_shub_master_enable(sensor, enable); +} + +static inline u32 st_lsm6dso16is_get_unaligned_le24(const u8 *p) +{ + return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; +} + +/** + * Single sensor read operation [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param ch: IIO Channel. + * @param val: Output data register value. + * @return IIO_VAL_INT if OK, negative value for ERROR + */ +static int +st_lsm6dso16is_shub_read_oneshot(struct st_lsm6dso16is_sensor *sensor, + struct iio_chan_spec const *ch, int *val) +{ + int err, delay, len = ch->scan_type.realbits >> 3; + u8 data[len]; + + err = st_lsm6dso16is_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1100000000 / sensor->mhz; + usleep_range(delay, delay + (delay >> 1)); + + err = st_lsm6dso16is_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + st_lsm6dso16is_shub_set_enable(sensor, false); + + switch (len) { + case 3: + *val = (s32)st_lsm6dso16is_get_unaligned_le24(data); + break; + case 2: + *val = (s16)get_unaligned_le16(data); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +/** + * Read Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param ch: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_lsm6dso16is_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dso16is_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_lsm6dso16is_shub_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->mhz; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * Write Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_lsm6dso16is_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + val = val * 1000 + val2 / 1000; + err = st_lsm6dso16is_shub_get_odr_val(sensor, val, &data); + if (!err) + sensor->mhz = val; + break; + } + case IIO_CHAN_INFO_SCALE: + err = 0; + break; + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Get a list of available sensor ODR [SHUB] + * + * List of available ODR returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_lsm6dso16is_sysfs_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) { + u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].mhz; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ", + val / 1000, val % 1000); + } + buf[len - 1] = '\n'; + + return len; +} + +/** + * Get a list of available sensor Full Scale [SHUB] + * + * List of available Full Scale returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t st_lsm6dso16is_sysfs_shub_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->fs_table.fs_len; i++) { + u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dso16is_sysfs_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_ext_scale_available, 0444, + st_lsm6dso16is_sysfs_shub_scale_avail, NULL, 0); + +static struct attribute *st_lsm6dso16is_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_ext_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dso16is_ext_attribute_group = { + .attrs = st_lsm6dso16is_ext_attributes, +}; + +static const struct iio_info st_lsm6dso16is_ext_info = { + .attrs = &st_lsm6dso16is_ext_attribute_group, + .read_raw = st_lsm6dso16is_shub_read_raw, + .write_raw = st_lsm6dso16is_shub_write_raw, +}; + +/** + * Allocate IIO device [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @param ext_settings: xternal sensor descritor entry. + * @param id: Sensor Identifier. + * @param i2c_addr: external I2C address on master bus. + * @return struct iio_dev *, NULL if ERROR + */ +static struct iio_dev *st_lsm6dso16is_shub_alloc_iio_dev(struct st_lsm6dso16is_hw *hw, + const struct st_lsm6dso16is_ext_dev_settings *ext_settings, + enum st_lsm6dso16is_sensor_id id, u8 i2c_addr) +{ + struct st_lsm6dso16is_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; + iio_dev->info = &st_lsm6dso16is_ext_info; + iio_dev->channels = ext_settings->ext_channels; + iio_dev->num_channels = ext_settings->ext_chan_depth; + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->mhz = ext_settings->odr_table.odr_avl[0].mhz; + sensor->gain = ext_settings->fs_table.fs_avl[0].gain; + sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr; + sensor->ext_dev_info.ext_dev_settings = ext_settings; + + switch (iio_dev->channels[0].type) { + case IIO_MAGN: + scnprintf(sensor->name, sizeof(sensor->name), "%s_magn", + ST_LSM6DSO16IS_DEV_NAME); + break; + case IIO_PRESSURE: + scnprintf(sensor->name, sizeof(sensor->name), "%s_press", + ST_LSM6DSO16IS_DEV_NAME); + break; + default: + scnprintf(sensor->name, sizeof(sensor->name), "%s_ext", + ST_LSM6DSO16IS_DEV_NAME); + break; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static int st_lsm6dso16is_shub_init_remote_sensor(struct st_lsm6dso16is_sensor *sensor) +{ + struct st_lsm6dso16is_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err = 0; + + if (ext_info->ext_dev_settings->bdu_reg.addr) + err = st_lsm6dso16is_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->bdu_reg.addr, + ext_info->ext_dev_settings->bdu_reg.mask, 1); + + if (ext_info->ext_dev_settings->temp_comp_reg.addr) + err = st_lsm6dso16is_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->temp_comp_reg.addr, + ext_info->ext_dev_settings->temp_comp_reg.mask, 1); + + if (ext_info->ext_dev_settings->off_canc_reg.addr) + err = st_lsm6dso16is_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->off_canc_reg.addr, + ext_info->ext_dev_settings->off_canc_reg.mask, 1); + + return err; +} + +/** + * Probe device function [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @return 0 if OK, negative for ERROR + */ +int st_lsm6dso16is_shub_probe(struct st_lsm6dso16is_hw *hw) +{ + const struct st_lsm6dso16is_ext_dev_settings *settings; + struct st_lsm6dso16is_sensor *acc_sensor, *sensor; + struct device_node *np = hw->dev->of_node; + u8 config[3], data, num_ext_dev = 0; + enum st_lsm6dso16is_sensor_id id; + int err, i = 0, j; + + if (np && of_property_read_bool(np, "drive-pullup-shub")) { + dev_info(hw->dev, "enabling pull up on i2c master\n"); + err = st_lsm6dso16is_shub_read_reg(hw, + ST_LSM6DSO16IS_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + if (err < 0) + return err; + + data |= ST_LSM6DSO16IS_SHUB_PU_EN_MASK; + err = st_lsm6dso16is_shub_write_reg(hw, + ST_LSM6DSO16IS_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + + if (err < 0) + return err; + + hw->i2c_master_pu = ST_LSM6DSO16IS_SHUB_PU_EN_MASK; + } + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSO16IS_ID_ACC]); + while (i < ARRAY_SIZE(st_lsm6dso16is_ext_dev_table) && + num_ext_dev < ST_LSM6DSO16IS_MAX_SLV_NUM) { + settings = &st_lsm6dso16is_ext_dev_table[i]; + + for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) { + if (!settings->i2c_addr[j]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[j] << 1) | 1; + config[1] = settings->wai_addr; + config[2] = 1; + + err = st_lsm6dso16is_shub_write_reg(hw, + ST_LSM6DSO16IS_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dso16is_shub_master_enable(acc_sensor, + true); + if (err < 0) + return err; + + st_lsm6dso16is_shub_wait_complete(hw); + + err = st_lsm6dso16is_shub_read_reg(hw, + ST_LSM6DSO16IS_REG_SENSOR_HUB_1_ADDR, + &data, sizeof(data)); + + st_lsm6dso16is_shub_master_enable(acc_sensor, false); + + if (err < 0) + return err; + + if (data != settings->wai_val) + continue; + + id = ST_LSM6DSO16IS_ID_EXT0 + num_ext_dev; + hw->iio_devs[id] = st_lsm6dso16is_shub_alloc_iio_dev(hw, + settings, id, + settings->i2c_addr[j]); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_lsm6dso16is_shub_init_remote_sensor(sensor); + if (err < 0) + return err; + + num_ext_dev++; + hw->ext_data_len += settings->data_len; + break; + } + + i++; + } + + if (!num_ext_dev) + return 0; + + memset(config, 0, sizeof(config)); + err = st_lsm6dso16is_shub_write_reg(hw, ST_LSM6DSO16IS_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + /* AuxSens = 3 + wr once */ + data = ST_LSM6DSO16IS_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + return st_lsm6dso16is_shub_write_reg(hw, + ST_LSM6DSO16IS_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); +} diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_spi.c b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_spi.c new file mode 100644 index 000000000000..d948e6fa31f7 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_spi.c @@ -0,0 +1,63 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dso16is spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dso16is.h" + +static const struct regmap_config st_lsm6dso16is_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dso16is_spi_probe(struct spi_device *spi) +{ + struct regmap *regmap; + + regmap = devm_regmap_init_spi(spi, &st_lsm6dso16is_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_lsm6dso16is_probe(&spi->dev, spi->irq, regmap); +} + +static const struct of_device_id st_lsm6dso16is_spi_of_match[] = { + { .compatible = "st,lsm6dso16is", }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dso16is_spi_of_match); + +static const struct spi_device_id st_lsm6dso16is_spi_id_table[] = { + { ST_LSM6DSO16IS_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dso16is_spi_id_table); + +static struct spi_driver st_lsm6dso16is_driver = { + .driver = { + .name = "st_" ST_LSM6DSO16IS_DEV_NAME "_spi", + .pm = &st_lsm6dso16is_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dso16is_spi_of_match), + }, + .probe = st_lsm6dso16is_spi_probe, + .id_table = st_lsm6dso16is_spi_id_table, +}; +module_spi_driver(st_lsm6dso16is_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dso16is spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_triggers.c b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_triggers.c new file mode 100644 index 000000000000..ffedc0077020 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dso16is/st_lsm6dso16is_triggers.c @@ -0,0 +1,132 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dso16is trigger buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dso16is.h" + +#define ST_LSM6DSO16IS_AG_SAMPLE_SIZE 6 +#define ST_LSM6DSO16IS_PT_SAMPLE_SIZE 2 + +static int st_lsm6dso16is_buffer_enable(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dso16is_sensor *sensor = iio_priv(iio_dev); + + if (sensor->id == ST_LSM6DSO16IS_ID_EXT0 || + sensor->id == ST_LSM6DSO16IS_ID_EXT1) + return st_lsm6dso16is_shub_set_enable(sensor, enable); + + return st_lsm6dso16is_sensor_set_enable(sensor, enable); +} + +static int st_lsm6dso16is_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dso16is_buffer_enable(iio_dev, true); +} + +static int st_lsm6dso16is_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dso16is_buffer_enable(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dso16is_buffer_setup_ops = { + .preenable = st_lsm6dso16is_fifo_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_lsm6dso16is_fifo_postdisable, +}; + +static irqreturn_t st_lsm6dso16is_buffer_pollfunc(int irq, void *private) +{ + u8 iio_buf[ALIGN(ST_LSM6DSO16IS_AG_SAMPLE_SIZE, sizeof(s64)) + + sizeof(s64) + sizeof(s64)]; + struct iio_poll_func *pf = private; + struct iio_dev *indio_dev = pf->indio_dev; + struct st_lsm6dso16is_sensor *sensor = iio_priv(indio_dev); + struct st_lsm6dso16is_hw *hw = sensor->hw; + int addr = indio_dev->channels[0].address; + + switch (indio_dev->channels[0].type) { + case IIO_ACCEL: + case IIO_ANGL_VEL: + st_lsm6dso16is_read_locked(hw, addr, &iio_buf, + ST_LSM6DSO16IS_AG_SAMPLE_SIZE); + break; + case IIO_TEMP: + st_lsm6dso16is_read_locked(hw, addr, &iio_buf, + ST_LSM6DSO16IS_PT_SAMPLE_SIZE); + break; + case IIO_PRESSURE: + st_lsm6dso16is_shub_read(sensor, addr, (u8 *)&iio_buf, + ST_LSM6DSO16IS_PT_SAMPLE_SIZE); + break; + case IIO_MAGN: + st_lsm6dso16is_shub_read(sensor, addr, (u8 *)&iio_buf, + ST_LSM6DSO16IS_AG_SAMPLE_SIZE); + break; + default: + return -EINVAL; + } + + iio_push_to_buffers_with_timestamp(indio_dev, iio_buf, + iio_get_time_ns(indio_dev)); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static int st_lsm6dso16is_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct st_lsm6dso16is_hw *hw = iio_trigger_get_drvdata(trig); + + dev_dbg(hw->dev, "trigger set %d\n", state); + + return 0; +} + +static const struct iio_trigger_ops st_lsm6dso16is_trigger_ops = { + .set_trigger_state = st_lsm6dso16is_trig_set_state, +}; + +int st_lsm6dso16is_allocate_buffers(struct st_lsm6dso16is_hw *hw) +{ + int i; + + for (i = 0; + i < ARRAY_SIZE(st_lsm6dso16is_triggered_main_sensor_list); + i++) { + enum st_lsm6dso16is_sensor_id id; + int err; + + id = st_lsm6dso16is_triggered_main_sensor_list[i]; + if (!hw->iio_devs[id]) + continue; + + err = devm_iio_triggered_buffer_setup(hw->dev, + hw->iio_devs[id], NULL, + st_lsm6dso16is_buffer_pollfunc, + &st_lsm6dso16is_buffer_setup_ops); + if (err) + return err; + } + + return 0; +} diff --git a/drivers/iio/stm/imu/st_lsm6dsox/Kconfig b/drivers/iio/stm/imu/st_lsm6dsox/Kconfig new file mode 100644 index 000000000000..2b75dda14315 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/Kconfig @@ -0,0 +1,38 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM6DSOX + tristate "STMicroelectronics LSM6DSOX sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_ST_LSM6DSOX_I2C if (I2C) + select IIO_ST_LSM6DSOX_SPI if (SPI_MASTER) + select IIO_ST_LSM6DSOX_I3C if (I3C) + help + Say yes here to build support for STMicroelectronics + LSM6DSO/LSM6DSOX/LSM6DSO32/LSM6DSO32X imu sensors. + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsox. + +config IIO_ST_LSM6DSOX_I2C + tristate + select REGMAP_I2C + depends on IIO_ST_LSM6DSOX + +config IIO_ST_LSM6DSOX_SPI + tristate + select REGMAP_SPI + depends on IIO_ST_LSM6DSOX + +config IIO_ST_LSM6DSOX_I3C + tristate + depends on IIO_ST_LSM6DSOX + select REGMAP_I3C + +config IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP + bool "Enable async hw timestamp read" + depends on IIO_ST_LSM6DSOX + help + Enable async task that sends over hw timestamp events. + diff --git a/drivers/iio/stm/imu/st_lsm6dsox/Makefile b/drivers/iio/stm/imu/st_lsm6dsox/Makefile new file mode 100644 index 000000000000..a36665072e40 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/Makefile @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_lsm6dsox-y := st_lsm6dsox_core.o \ + st_lsm6dsox_buffer.o \ + st_lsm6dsox_mlc.o \ + st_lsm6dsox_shub.o \ + st_lsm6dsox_embfunc.o + +st_lsm6dsox-$(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) += st_lsm6dsox_hwtimestamp.o + +obj-$(CONFIG_IIO_ST_LSM6DSOX) += st_lsm6dsox.o +obj-$(CONFIG_IIO_ST_LSM6DSOX_I2C) += st_lsm6dsox_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSOX_SPI) += st_lsm6dsox_spi.o +obj-$(CONFIG_IIO_ST_LSM6DSOX_I3C) += st_lsm6dsox_i3c.o +obj-$(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) += st_lsm6dsox_hwtimestamp.o diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox.h b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox.h new file mode 100644 index 000000000000..c3ef2d83b19c --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox.h @@ -0,0 +1,864 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dsox sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSOX_H +#define ST_LSM6DSOX_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#define ST_LSM6DSOX_ODR_EXPAND(odr, uodr) (((odr) * 1000000) + (uodr)) + +#define ST_LSM6DSO_DEV_NAME "lsm6dso" +#define ST_LSM6DSOX_DEV_NAME "lsm6dsox" +#define ST_LSM6DSO32_DEV_NAME "lsm6dso32" +#define ST_LSM6DSO32X_DEV_NAME "lsm6dso32x" + +#define ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DSOX_REG_SHUB_REG_MASK BIT(6) +#define ST_LSM6DSOX_REG_FUNC_CFG_MASK BIT(7) +#define ST_LSM6DSOX_REG_ACCESS_MASK GENMASK(7, 6) + +#define ST_LSM6DSOX_REG_FIFO_CTRL1_ADDR 0x07 +#define ST_LSM6DSOX_REG_FIFO_CTRL2_ADDR 0x08 +#define ST_LSM6DSOX_REG_FIFO_WTM_MASK GENMASK(8, 0) +#define ST_LSM6DSOX_REG_FIFO_WTM8_MASK BIT(0) +#define ST_LSM6DSOX_REG_FIFO_STATUS_DIFF GENMASK(9, 0) + +#define ST_LSM6DSOX_REG_FIFO_CTRL3_ADDR 0x09 +#define ST_LSM6DSOX_REG_BDR_XL_MASK GENMASK(3, 0) +#define ST_LSM6DSOX_REG_BDR_GY_MASK GENMASK(7, 4) + +#define ST_LSM6DSOX_REG_FIFO_CTRL4_ADDR 0x0a +#define ST_LSM6DSOX_REG_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_LSM6DSOX_REG_ODR_T_BATCH_MASK GENMASK(5, 4) +#define ST_LSM6DSOX_REG_DEC_TS_MASK GENMASK(7, 6) + +#define ST_LSM6DSOX_REG_INT1_CTRL_ADDR 0x0d +#define ST_LSM6DSOX_REG_INT2_CTRL_ADDR 0x0e +#define ST_LSM6DSOX_REG_FIFO_TH_MASK BIT(3) + +#define ST_LSM6DSOX_REG_WHOAMI_ADDR 0x0f +#define ST_LSM6DSOX_WHOAMI_VAL 0x6c + +#define ST_LSM6DSOX_CTRL1_XL_ADDR 0x10 +#define ST_LSM6DSOX_CTRL2_G_ADDR 0x11 + +#define ST_LSM6DSOX_REG_CTRL3_C_ADDR 0x12 +#define ST_LSM6DSOX_REG_SW_RESET_MASK BIT(0) +#define ST_LSM6DSOX_REG_PP_OD_MASK BIT(4) +#define ST_LSM6DSOX_REG_H_LACTIVE_MASK BIT(5) +#define ST_LSM6DSOX_REG_BDU_MASK BIT(6) +#define ST_LSM6DSOX_REG_BOOT_MASK BIT(7) + +#define ST_LSM6DSOX_REG_CTRL4_C_ADDR 0x13 +#define ST_LSM6DSOX_REG_DRDY_MASK BIT(3) + +#define ST_LSM6DSOX_REG_CTRL5_C_ADDR 0x14 +#define ST_LSM6DSOX_REG_ROUNDING_MASK GENMASK(6, 5) +#define ST_LSM6DSOX_REG_ST_G_MASK GENMASK(3, 2) +#define ST_LSM6DSOX_REG_ST_XL_MASK GENMASK(1, 0) + +#define ST_LSM6DSOX_SELFTEST_ACCEL_MIN 737 +#define ST_LSM6DSOX_SELFTEST_ACCEL_MAX 13934 +#define ST_LSM6DSOX_SELFTEST_GYRO_MIN 2142 +#define ST_LSM6DSOX_SELFTEST_GYRO_MAX 10000 + +#define ST_LSM6DSOX_SELF_TEST_DISABLED_VAL 0 +#define ST_LSM6DSOX_SELF_TEST_POS_SIGN_VAL 1 +#define ST_LSM6DSOX_SELF_TEST_NEG_ACCEL_SIGN_VAL 2 +#define ST_LSM6DSOX_SELF_TEST_NEG_GYRO_SIGN_VAL 3 + +#define ST_LSM6DSOX_REG_STATUS_MASTER_MAINPAGE_ADDR 0x39 +#define ST_LSM6DSOX_REG_STATUS_SENS_HUB_ENDOP_MASK BIT(0) + +#define ST_LSM6DSOX_REG_CTRL6_C_ADDR 0x15 +#define ST_LSM6DSOX_REG_XL_HM_MODE_MASK BIT(4) + +#define ST_LSM6DSOX_REG_CTRL7_G_ADDR 0x16 +#define ST_LSM6DSOX_REG_G_HM_MODE_MASK BIT(7) + +#define ST_LSM6DSOX_REG_CTRL10_C_ADDR 0x19 +#define ST_LSM6DSOX_REG_TIMESTAMP_EN_MASK BIT(5) + +#define ST_LSM6DSOX_REG_STATUS_ADDR 0x1e +#define ST_LSM6DSOX_REG_STATUS_XLDA BIT(0) +#define ST_LSM6DSOX_REG_STATUS_GDA BIT(1) +#define ST_LSM6DSOX_REG_STATUS_TDA BIT(2) + +#define ST_LSM6DSOX_REG_OUT_TEMP_L_ADDR 0x20 + +#define ST_LSM6DSOX_REG_OUTX_L_A_ADDR 0x28 +#define ST_LSM6DSOX_REG_OUTY_L_A_ADDR 0x2a +#define ST_LSM6DSOX_REG_OUTZ_L_A_ADDR 0x2c + +#define ST_LSM6DSOX_REG_OUTX_L_G_ADDR 0x22 +#define ST_LSM6DSOX_REG_OUTY_L_G_ADDR 0x24 +#define ST_LSM6DSOX_REG_OUTZ_L_G_ADDR 0x26 + +#define ST_LSM6DSOX_REG_EMB_FUNC_STATUS_MAINPAGE 0x35 +#define ST_LSM6DSOX_REG_INT_STEP_DET_MASK BIT(3) +#define ST_LSM6DSOX_REG_INT_TILT_MASK BIT(4) +#define ST_LSM6DSOX_REG_INT_SIGMOT_MASK BIT(5) + +#define ST_LSM6DSOX_FSM_STATUS_A_MAINPAGE 0x36 +#define ST_LSM6DSOX_FSM_STATUS_B_MAINPAGE 0x37 +#define ST_LSM6DSOX_MLC_STATUS_MAINPAGE 0x38 + +#define ST_LSM6DSOX_REG_FIFO_STATUS1_ADDR 0x3a +#define ST_LSM6DSOX_REG_TIMESTAMP0_ADDR 0x40 +#define ST_LSM6DSOX_REG_TIMESTAMP2_ADDR 0x42 + +#define ST_LSM6DSOX_REG_TAP_CFG0_ADDR 0x56 +#define ST_LSM6DSOX_REG_LIR_MASK BIT(0) + +#define ST_LSM6DSOX_REG_MD1_CFG_ADDR 0x5e +#define ST_LSM6DSOX_REG_MD2_CFG_ADDR 0x5f +#define ST_LSM6DSOX_REG_INT2_TIMESTAMP_MASK BIT(0) +#define ST_LSM6DSOX_REG_INT_EMB_FUNC_MASK BIT(1) + +#define ST_LSM6DSOX_INTERNAL_FREQ_FINE 0x63 + +#define ST_LSM6DSOX_REG_FIFO_DATA_OUT_TAG_ADDR 0x78 + +/* shub registers */ +#define ST_LSM6DSOX_REG_MASTER_CONFIG_ADDR 0x14 +#define ST_LSM6DSOX_REG_WRITE_ONCE_MASK BIT(6) +#define ST_LSM6DSOX_REG_SHUB_PU_EN_MASK BIT(3) +#define ST_LSM6DSOX_REG_MASTER_ON_MASK BIT(2) + +#define ST_LSM6DSOX_REG_SLV0_ADDR 0x15 +#define ST_LSM6DSOX_REG_SLV0_CFG 0x17 +#define ST_LSM6DSOX_REG_SLV1_ADDR 0x18 +#define ST_LSM6DSOX_REG_SLV2_ADDR 0x1b +#define ST_LSM6DSOX_REG_SLV3_ADDR 0x1e +#define ST_LSM6DSOX_REG_DATAWRITE_SLV0_ADDR 0x21 +#define ST_LSM6DSOX_REG_BATCH_EXT_SENS_EN_MASK BIT(3) +#define ST_LSM6DSOX_REG_SLAVE_NUMOP_MASK GENMASK(2, 0) + +#define ST_LSM6DSOX_REG_STATUS_MASTER_ADDR 0x22 +#define ST_LSM6DSOX_REG_SENS_HUB_ENDOP_MASK BIT(0) + +#define ST_LSM6DSOX_REG_SLV0_OUT_ADDR 0x02 + +/* embedded function registers */ +#define ST_LSM6DSOX_EMB_FUNC_EN_A_ADDR 0x04 +#define ST_LSM6DSOX_PEDO_EN_MASK BIT(3) +#define ST_LSM6DSOX_TILT_EN_MASK BIT(4) +#define ST_LSM6DSOX_SIGN_MOTION_EN_MASK BIT(5) + +#define ST_LSM6DSOX_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_LSM6DSOX_FSM_EN_MASK BIT(0) +#define ST_LSM6DSOX_MLC_EN_MASK BIT(4) + +#define ST_LSM6DSOX_EMB_FUNC_INT1_ADDR 0x0a +#define ST_LSM6DSOX_INT_STEP_DET_MASK BIT(3) +#define ST_LSM6DSOX_INT_TILT_MASK BIT(4) +#define ST_LSM6DSOX_INT_SIGMOT_MASK BIT(5) + +#define ST_LSM6DSOX_FSM_INT1_A_ADDR 0x0b +#define ST_LSM6DSOX_FSM_INT1_B_ADDR 0x0c +#define ST_LSM6DSOX_MLC_INT1_ADDR 0x0d +#define ST_LSM6DSOX_EMB_FUNC_INT2_ADDR 0x0e + +#define ST_LSM6DSOX_FSM_INT2_A_ADDR 0x0f +#define ST_LSM6DSOX_FSM_INT2_B_ADDR 0x10 +#define ST_LSM6DSOX_MLC_INT2_ADDR 0x11 + +#define ST_LSM6DSOX_REG_MLC_STATUS_ADDR 0x15 + +#define ST_LSM6DSOX_PAGE_RW_ADDR 0x17 +#define ST_LSM6DSOX_REG_EMB_FUNC_LIR_MASK BIT(7) + +#define ST_LSM6DSOX_EMB_FUNC_FIFO_CFG_ADDR 0x44 +#define ST_LSM6DSOX_PEDO_FIFO_EN_MASK BIT(6) + +#define ST_LSM6DSOX_FSM_ENABLE_A_ADDR 0x46 +#define ST_LSM6DSOX_FSM_ENABLE_B_ADDR 0x47 + +#define ST_LSM6DSOX_FSM_OUTS1_ADDR 0x4c + +#define ST_LSM6DSOX_REG_STEP_COUNTER_L_ADDR 0x62 +#define ST_LSM6DSOX_REG_EMB_FUNC_SRC_ADDR 0x64 +#define ST_LSM6DSOX_REG_PEDO_RST_STEP_MASK BIT(7) + +#define ST_LSM6DSOX_REG_MLC0_SRC_ADDR 0x70 + +/* Timestamp Tick 25us/LSB */ +#define ST_LSM6DSOX_TS_DELTA_NS 25000ULL + +/* Temperature in uC */ +#define ST_LSM6DSOX_TEMP_GAIN 256 +#define ST_LSM6DSOX_TEMP_OFFSET 6400 + +/* FIFO simple size and depth */ +#define ST_LSM6DSOX_SAMPLE_SIZE 6 +#define ST_LSM6DSOX_TS_SAMPLE_SIZE 4 +#define ST_LSM6DSOX_TAG_SIZE 1 +#define ST_LSM6DSOX_FIFO_SAMPLE_SIZE (ST_LSM6DSOX_SAMPLE_SIZE + \ + ST_LSM6DSOX_TAG_SIZE) +#define ST_LSM6DSOX_MAX_FIFO_DEPTH 416 + +enum st_lsm6dsox_hw_id { + ST_LSM6DSO_ID, + ST_LSM6DSOX_ID, + ST_LSM6DSO32_ID, + ST_LSM6DSO32X_ID, + ST_LSM6DSOX_MAX_ID, +}; + +#define ST_LSM6DSOX_DEFAULT_KTIME (200000000) +#define ST_LSM6DSOX_FAST_KTIME (5000000) + +#define ST_LSM6DSOX_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, \ +} + +static const struct iio_event_spec st_lsm6dsox_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_lsm6dsox_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_LSM6DSOX_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lsm6dsox_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_LSM6DSOX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +enum st_lsm6dsox_pm_t { + ST_LSM6DSOX_HP_MODE = 0, + ST_LSM6DSOX_LP_MODE, + ST_LSM6DSOX_NO_MODE, +}; + +enum st_lsm6dsox_fsm_mlc_enable_id { + ST_LSM6DSOX_MLC_FSM_DISABLED = 0, + ST_LSM6DSOX_MLC_ENABLED = BIT(0), + ST_LSM6DSOX_FSM_ENABLED = BIT(1), +}; + +/** + * struct mlc_config_t - MLC/FSM data register structure + * @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_lsm6dsox_mlc_config_t { + uint8_t mlc_int_addr; + uint8_t mlc_int_mask; + uint8_t fsm_int_addr[2]; + uint8_t fsm_int_mask[2]; + uint8_t mlc_configured; + uint8_t fsm_configured; + uint16_t bin_len; + uint16_t requested_odr; + enum st_lsm6dsox_fsm_mlc_enable_id status; +}; + +/** + * struct st_lsm6dsox_reg - Generic sensor register + * description (addr + mask) + * + * @addr: Address of register. + * @mask: Bitmask register for proper usage. + */ +struct st_lsm6dsox_reg { + u8 addr; + u8 mask; +}; + +enum st_lsm6dsox_suspend_resume_register { + ST_LSM6DSOX_CTRL1_XL_REG = 0, + ST_LSM6DSOX_CTRL2_G_REG, + ST_LSM6DSOX_REG_CTRL3_C_REG, + ST_LSM6DSOX_REG_CTRL4_C_REG, + ST_LSM6DSOX_REG_CTRL5_C_REG, + ST_LSM6DSOX_REG_CTRL10_C_REG, + ST_LSM6DSOX_REG_TAP_CFG0_REG, + ST_LSM6DSOX_REG_INT1_CTRL_REG, + ST_LSM6DSOX_REG_INT2_CTRL_REG, + ST_LSM6DSOX_REG_FIFO_CTRL1_REG, + ST_LSM6DSOX_REG_FIFO_CTRL2_REG, + ST_LSM6DSOX_REG_FIFO_CTRL3_REG, + ST_LSM6DSOX_REG_FIFO_CTRL4_REG, + ST_LSM6DSOX_REG_EMB_FUNC_EN_B_REG, + ST_LSM6DSOX_REG_FSM_INT1_A_REG, + ST_LSM6DSOX_REG_FSM_INT1_B_REG, + ST_LSM6DSOX_REG_MLC_INT1_REG, + ST_LSM6DSOX_REG_FSM_INT2_A_REG, + ST_LSM6DSOX_REG_FSM_INT2_B_REG, + ST_LSM6DSOX_REG_MLC_INT2_REG, + ST_LSM6DSOX_SUSPEND_RESUME_REGS, +}; + +/** + * Define embedded functions register access + * + * FUNC_CFG_ACCESS_0 is default bank + * FUNC_CFG_ACCESS_SHUB_REG Enable access to the sensor hub (I2C master) + * registers. + * FUNC_CFG_ACCESS_FUNC_CFG Enable access to the embedded functions + * configuration registers. + */ +enum st_lsm6dsox_page_sel_register { + FUNC_CFG_ACCESS_0 = 0, + FUNC_CFG_ACCESS_SHUB_REG, + FUNC_CFG_ACCESS_FUNC_CFG, +}; + +/** + * struct st_lsm6dsox_suspend_resume_entry - Register value for backup/restore + * @page: Page bank reg map. + * @addr: Address of register. + * @val: Register value. + * @mask: Bitmask register for proper usage. + */ +struct st_lsm6dsox_suspend_resume_entry { + u8 page; + u8 addr; + u8 val; + u8 mask; +}; + +/** + * struct st_lsm6dsox_odr - Single ODR entry + * @hz: Most significant part of the sensor ODR (Hz). + * @uhz: Less significant part of the sensor ODR (micro Hz). + * @val: ODR register value. + * @batch_val: Batching ODR register value. + */ +struct st_lsm6dsox_odr { + u16 hz; + u32 uhz; + u8 val; + u8 batch_val; +}; + +/** + * struct st_lsm6dsox_odr_table_entry - Sensor ODR table + * @size: Size of ODR table. + * @reg: ODR register. + * @pm: Power mode register. + * @batching_reg: ODR register for batching on fifo. + * @odr_avl: Array of supported ODR value. + */ +struct st_lsm6dsox_odr_table_entry { + u8 size; + struct st_lsm6dsox_reg reg; + struct st_lsm6dsox_reg pm; + struct st_lsm6dsox_reg batching_reg; + struct st_lsm6dsox_odr odr_avl[8]; +}; + +/** + * struct st_lsm6dsox_fs + * brief Full scale entry + * + * @gain: The gain to obtain data value from raw data (LSB). + * @val: Register value. + */ +struct st_lsm6dsox_fs { + u32 gain; + u8 val; +}; + +/** + * struct st_lsm6dsox_fs_table_entry - Full Scale sensor table + * @reg: st_lsm6dsox_reg struct. + * @fs_avl: Full Scale list entries. + * @fs_len: Real size of fs_avl array. + */ +#define ST_LSM6DSOX_FS_LIST_SIZE 4 +struct st_lsm6dsox_fs_table_entry { + struct st_lsm6dsox_reg reg; + struct st_lsm6dsox_fs fs_avl[ST_LSM6DSOX_FS_LIST_SIZE]; + int fs_len; +}; + +enum st_lsm6dsox_sensor_id { + ST_LSM6DSOX_ID_GYRO = 0, + ST_LSM6DSOX_ID_ACC, + ST_LSM6DSOX_ID_TEMP, + ST_LSM6DSOX_ID_EXT0, + ST_LSM6DSOX_ID_EXT1, + ST_LSM6DSOX_ID_STEP_COUNTER, + ST_LSM6DSOX_ID_STEP_DETECTOR, + ST_LSM6DSOX_ID_SIGN_MOTION, + ST_LSM6DSOX_ID_TILT, + ST_LSM6DSOX_ID_MLC, + ST_LSM6DSOX_ID_MLC_0, + ST_LSM6DSOX_ID_MLC_1, + ST_LSM6DSOX_ID_MLC_2, + ST_LSM6DSOX_ID_MLC_3, + ST_LSM6DSOX_ID_MLC_4, + ST_LSM6DSOX_ID_MLC_5, + ST_LSM6DSOX_ID_MLC_6, + ST_LSM6DSOX_ID_MLC_7, + ST_LSM6DSOX_ID_FSM_0, + ST_LSM6DSOX_ID_FSM_1, + ST_LSM6DSOX_ID_FSM_2, + ST_LSM6DSOX_ID_FSM_3, + ST_LSM6DSOX_ID_FSM_4, + ST_LSM6DSOX_ID_FSM_5, + ST_LSM6DSOX_ID_FSM_6, + ST_LSM6DSOX_ID_FSM_7, + ST_LSM6DSOX_ID_FSM_8, + ST_LSM6DSOX_ID_FSM_9, + ST_LSM6DSOX_ID_FSM_10, + ST_LSM6DSOX_ID_FSM_11, + ST_LSM6DSOX_ID_FSM_12, + ST_LSM6DSOX_ID_FSM_13, + ST_LSM6DSOX_ID_FSM_14, + ST_LSM6DSOX_ID_FSM_15, + ST_LSM6DSOX_ID_MAX, +}; + +/** + * @enum st_lsm6dso_sensor_id + * @brief Sensor Table Identifier + */ +static const enum st_lsm6dsox_sensor_id st_lsm6dsox_main_sensor_list[] = { + [0] = ST_LSM6DSOX_ID_GYRO, + [1] = ST_LSM6DSOX_ID_ACC, + [2] = ST_LSM6DSOX_ID_TEMP, + [3] = ST_LSM6DSOX_ID_STEP_COUNTER, + [4] = ST_LSM6DSOX_ID_STEP_DETECTOR, + [5] = ST_LSM6DSOX_ID_SIGN_MOTION, + [6] = ST_LSM6DSOX_ID_TILT, +}; + +static const enum st_lsm6dsox_sensor_id st_lsm6dsox_mlc_sensor_list[] = { + [0] = ST_LSM6DSOX_ID_MLC_0, + [1] = ST_LSM6DSOX_ID_MLC_1, + [2] = ST_LSM6DSOX_ID_MLC_2, + [3] = ST_LSM6DSOX_ID_MLC_3, + [4] = ST_LSM6DSOX_ID_MLC_4, + [5] = ST_LSM6DSOX_ID_MLC_5, + [6] = ST_LSM6DSOX_ID_MLC_6, + [7] = ST_LSM6DSOX_ID_MLC_7, +}; + +static const enum st_lsm6dsox_sensor_id st_lsm6dsox_fsm_sensor_list[] = { + [0] = ST_LSM6DSOX_ID_FSM_0, + [1] = ST_LSM6DSOX_ID_FSM_1, + [2] = ST_LSM6DSOX_ID_FSM_2, + [3] = ST_LSM6DSOX_ID_FSM_3, + [4] = ST_LSM6DSOX_ID_FSM_4, + [5] = ST_LSM6DSOX_ID_FSM_5, + [6] = ST_LSM6DSOX_ID_FSM_6, + [7] = ST_LSM6DSOX_ID_FSM_7, + [8] = ST_LSM6DSOX_ID_FSM_8, + [9] = ST_LSM6DSOX_ID_FSM_9, + [10] = ST_LSM6DSOX_ID_FSM_10, + [11] = ST_LSM6DSOX_ID_FSM_11, + [12] = ST_LSM6DSOX_ID_FSM_12, + [13] = ST_LSM6DSOX_ID_FSM_13, + [14] = ST_LSM6DSOX_ID_FSM_14, + [15] = ST_LSM6DSOX_ID_FSM_15, +}; + +#define ST_LSM6DSOX_ID_ALL_FSM_MLC (BIT(ST_LSM6DSOX_ID_MLC_0) | \ + BIT(ST_LSM6DSOX_ID_MLC_1) | \ + BIT(ST_LSM6DSOX_ID_MLC_2) | \ + BIT(ST_LSM6DSOX_ID_MLC_3) | \ + BIT(ST_LSM6DSOX_ID_MLC_4) | \ + BIT(ST_LSM6DSOX_ID_MLC_5) | \ + BIT(ST_LSM6DSOX_ID_MLC_6) | \ + BIT(ST_LSM6DSOX_ID_MLC_7) | \ + BIT(ST_LSM6DSOX_ID_FSM_0) | \ + BIT(ST_LSM6DSOX_ID_FSM_1) | \ + BIT(ST_LSM6DSOX_ID_FSM_2) | \ + BIT(ST_LSM6DSOX_ID_FSM_3) | \ + BIT(ST_LSM6DSOX_ID_FSM_4) | \ + BIT(ST_LSM6DSOX_ID_FSM_5) | \ + BIT(ST_LSM6DSOX_ID_FSM_6) | \ + BIT(ST_LSM6DSOX_ID_FSM_7) | \ + BIT(ST_LSM6DSOX_ID_FSM_8) | \ + BIT(ST_LSM6DSOX_ID_FSM_9) | \ + BIT(ST_LSM6DSOX_ID_FSM_10) | \ + BIT(ST_LSM6DSOX_ID_FSM_11) | \ + BIT(ST_LSM6DSOX_ID_FSM_12) | \ + BIT(ST_LSM6DSOX_ID_FSM_13) | \ + BIT(ST_LSM6DSOX_ID_FSM_14) | \ + BIT(ST_LSM6DSOX_ID_FSM_15)) + +/* HW devices that can wakeup the target */ +#define ST_LSM6DSOX_WAKE_UP_SENSORS (BIT(ST_LSM6DSOX_ID_GYRO) | \ + BIT(ST_LSM6DSOX_ID_ACC)) + +/* this is the minimal ODR for wake-up sensors and dependencies */ +#define ST_LSM6DSOX_MIN_ODR_IN_WAKEUP 26 + +enum st_lsm6dsox_fifo_mode { + ST_LSM6DSOX_FIFO_BYPASS = 0x0, + ST_LSM6DSOX_FIFO_CONT = 0x6, +}; + +enum { + ST_LSM6DSOX_HW_FLUSH, + ST_LSM6DSOX_HW_OPERATIONAL, +}; + +struct st_lsm6dsox_ext_dev_info { + const struct st_lsm6dsox_ext_dev_settings *ext_dev_settings; + u8 ext_dev_i2c_addr; +}; + +/** + * struct st_lsm6dsox_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dsox_hw. + * @ext_dev_info: For sensor hub indicate device info struct. + * @odr: Output data rate of the sensor [Hz]. + * @uodr: Output data rate of the sensor [uHz]. + * @gain: Configured sensor sensitivity. + * @offset: Sensor data offset. + * @decimator: Sensor decimator + * @dec_counter: Sensor decimator counter + * @old_data: Used by Temperature sensor for data comtinuity. + * @max_watermark: Max supported watermark level. + * @watermark: Sensor watermark level. + * @pm: sensor power mode (HP, LP). + * @last_fifo_timestamp: Timestamp related to last sample in FIFO. + * @selftest_status: Report last self test status. + * @min_st: Min self test raw data value. + * @max_st: Max self test raw data value. + * @status_reg: Status register used by mlc/fsm. + * @outreg_addr: Output data register used by mlc/fsm. + * @status: Status of mlc/fsm algos. + */ +struct st_lsm6dsox_sensor { + char name[32]; + enum st_lsm6dsox_sensor_id id; + struct st_lsm6dsox_hw *hw; + struct st_lsm6dsox_ext_dev_info ext_dev_info; + + int odr; + int uodr; + + union { + struct { + u32 gain; + u32 offset; + u8 decimator; + u8 dec_counter; + __le16 old_data; + u16 max_watermark; + u16 watermark; + enum st_lsm6dsox_pm_t pm; + s64 last_fifo_timestamp; + + /* self test */ + int8_t selftest_status; + int min_st; + int max_st; + }; + struct { + uint8_t status_reg; + uint8_t outreg_addr; + enum st_lsm6dsox_fsm_mlc_enable_id status; + }; + }; +}; + +/** + * struct st_lsm6dsox_hw - ST IMU MEMS hw instance + * @dev_name: STM device name. + * @dev: Pointer to instance of struct device (I2C or SPI). + * @irq: Device interrupt line (I2C or SPI). + * @regmap: Register map of the device. + * @page_lock: Mutex to prevent concurrent access to the page selector. + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO. + * @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. + * @ext_data_len: Number of i2c slave devices connected to I2C master. + * @ts_delta_ns: Calibrated delta timestamp. + * @ts_offset: Hw timestamp offset. + * @hw_ts: Latest hw timestamp from the sensor. + * @tsample: Sample timestamp. + * @delta_ts: Delta time between two consecutive interrupts. + * @ts: Latest timestamp from irq handler. + * @i2c_master_pu: I2C master line Pull Up configuration. + * @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. + * @settings: ST IMU sensor settings. + * @st_lsm6dsox_odr_table: Sensors ODR table. + * @preload_mlc: MLC/FSM preload flag. + * @iio_devs: Pointers to acc/gyro iio_dev instances. + * @embfunc_irq_reg: Embedded function irq configuration register (other). + * @embfunc_pg0_irq_reg: Embedded function irq configuration register (page 0). + */ +struct st_lsm6dsox_hw { + char dev_name[16]; + struct device *dev; + int irq; + struct regmap *regmap; + struct mutex page_lock; + struct mutex fifo_lock; + enum st_lsm6dsox_fifo_mode fifo_mode; + unsigned long state; + u64 enable_mask; + s64 hw_timestamp_global; + +#if defined(CONFIG_IIO_ST_LSM6DSOX_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_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + + u8 ext_data_len; + u64 ts_delta_ns; + s64 ts_offset; + s64 hw_ts; + s64 tsample; + s64 delta_ts; + s64 ts; + u8 i2c_master_pu; + u32 module_id; + struct iio_mount_matrix orientation; + struct regulator *vdd_supply; + struct regulator *vddio_supply; + + struct st_lsm6dsox_mlc_config_t *mlc_config; + const struct st_lsm6dsox_settings *settings; + const struct st_lsm6dsox_odr_table_entry *st_lsm6dsox_odr_table; + + bool preload_mlc; + + struct iio_dev *iio_devs[ST_LSM6DSOX_ID_MAX]; + + u8 embfunc_irq_reg; + u8 embfunc_pg0_irq_reg; +}; + +extern const struct dev_pm_ops st_lsm6dsox_pm_ops; + +/** + * struct st_lsm6dsox_settings - ST IMU sensor settings + * + * @hw_id: Hw id supported by the driver configuration. + * @name: Device name supported by the driver configuration. + * @fs_table: Full scale table for a selected device. + * @st_mlc_probe: MLC probe flag. + * @st_fsm_probe: FSM probe flag. + */ +struct st_lsm6dsox_settings { + struct { + enum st_lsm6dsox_hw_id hw_id; + const char *name; + } id[ST_LSM6DSOX_MAX_ID]; + struct st_lsm6dsox_fs_table_entry fs_table[ST_LSM6DSOX_ID_MAX]; + bool st_mlc_probe; + bool st_fsm_probe; +}; + + +static inline bool +st_lsm6dsox_is_fifo_enabled(struct st_lsm6dsox_hw *hw) +{ + return hw->enable_mask & (BIT(ST_LSM6DSOX_ID_GYRO) | + BIT(ST_LSM6DSOX_ID_STEP_COUNTER) | + BIT(ST_LSM6DSOX_ID_ACC)); +} + +static inline bool st_lsm6dsox_run_mlc_task(struct st_lsm6dsox_hw *hw) +{ + return hw->settings->st_mlc_probe || hw->settings->st_fsm_probe; +} + +static inline int __st_lsm6dsox_write_with_mask(struct st_lsm6dsox_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_LSM6DSOX_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int +st_lsm6dsox_update_bits_locked(struct st_lsm6dsox_hw *hw, unsigned int addr, + unsigned int mask, unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dsox_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +/* use when mask is constant */ +static inline int +st_lsm6dsox_write_with_mask_locked(struct st_lsm6dsox_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = FIELD_PREP(mask, data); + + mutex_lock(&hw->page_lock); + err = regmap_update_bits(hw->regmap, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsox_read_locked(struct st_lsm6dsox_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsox_write_locked(struct st_lsm6dsox_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_lsm6dsox_set_page_access(struct st_lsm6dsox_hw *hw, + unsigned int val, + unsigned int mask) +{ + return regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR, + mask, + ST_LSM6DSOX_SHIFT_VAL(val, mask)); +} + +int st_lsm6dsox_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap); +int st_lsm6dsox_sensor_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable); +int st_lsm6dsox_buffers_setup(struct st_lsm6dsox_hw *hw); +int st_lsm6dsox_get_batch_val(struct st_lsm6dsox_sensor *sensor, + int odr, int uodr, u8 *val); +int st_lsm6dsox_update_watermark(struct st_lsm6dsox_sensor *sensor, + u16 watermark); +ssize_t st_lsm6dsox_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsox_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsox_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsox_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsox_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +int st_lsm6dsox_suspend_fifo(struct st_lsm6dsox_hw *hw); +int st_lsm6dsox_set_fifo_mode(struct st_lsm6dsox_hw *hw, + enum st_lsm6dsox_fifo_mode fifo_mode); +int st_lsm6dsox_update_batching(struct iio_dev *iio_dev, bool enable); +int st_lsm6dsox_of_get_pin(struct st_lsm6dsox_hw *hw, int *pin); +int st_lsm6dsox_shub_probe(struct st_lsm6dsox_hw *hw); +int st_lsm6dsox_shub_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable); + +#if defined(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) +int st_lsm6dsox_hwtimesync_init(struct st_lsm6dsox_hw *hw); +#else /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ +static inline int +st_lsm6dsox_hwtimesync_init(struct st_lsm6dsox_hw *hw) +{ + return 0; +} +#endif /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + +int st_lsm6dsox_mlc_probe(struct st_lsm6dsox_hw *hw); +int st_lsm6dsox_mlc_remove(struct device *dev); +int st_lsm6dsox_mlc_check_status(struct st_lsm6dsox_hw *hw); +int st_lsm6dsox_mlc_init_preload(struct st_lsm6dsox_hw *hw); + +int st_lsm6dsox_embfunc_sensor_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable); +int st_lsm6dsox_step_counter_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable); +int st_lsm6dsox_reset_step_counter(struct iio_dev *iio_dev); +int st_lsm6dsox_embedded_function_init(struct st_lsm6dsox_hw *hw); +#endif /* ST_LSM6DSOX_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_buffer.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_buffer.c new file mode 100644 index 000000000000..dd9e15cdcd37 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_buffer.c @@ -0,0 +1,717 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsox.h" + +#define ST_LSM6DSOX_SAMPLE_DISCHARD 0x7ffd + +/* Timestamp convergence filter parameters */ +#define ST_LSM6DSOX_EWMA_LEVEL 120 +#define ST_LSM6DSOX_EWMA_DIV 128 + +#define ST_LSM6DSOX_TIMESTAMP_RESET_VALUE 0xaa + +/* FIFO tags */ +enum { + ST_LSM6DSOX_GYRO_TAG = 0x01, + ST_LSM6DSOX_ACC_TAG = 0x02, + ST_LSM6DSOX_TEMP_TAG = 0x03, + ST_LSM6DSOX_TS_TAG = 0x04, + ST_LSM6DSOX_EXT0_TAG = 0x0f, + ST_LSM6DSOX_EXT1_TAG = 0x10, + ST_LSM6DSOX_SC_TAG = 0x12, +}; + +/* Default timeout before to re-enable gyro */ +static int lsm6dsox_delay_gyro = 10; +module_param(lsm6dsox_delay_gyro, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); +MODULE_PARM_DESC(lsm6dsox_delay_gyro, "Delay for Gyro arming"); +static bool delayed_enable_gyro; + +static inline s64 st_lsm6dsox_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LSM6DSOX_EWMA_DIV - weight) * diff, + ST_LSM6DSOX_EWMA_DIV); + + return old + incr; +} + +static inline int st_lsm6dsox_reset_hwts(struct st_lsm6dsox_hw *hw) +{ + u8 data = ST_LSM6DSOX_TIMESTAMP_RESET_VALUE; + int ret; + + ret = st_lsm6dsox_write_locked(hw, ST_LSM6DSOX_REG_TIMESTAMP2_ADDR, + data); + if (ret < 0) + return ret; + +#if defined(CONFIG_IIO_ST_LSM6DSOX_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_LSM6DSOX_FAST_KTIME); +#else /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + hw->hw_timestamp_global = (hw->hw_timestamp_global + (1LL << 32)) & + GENMASK_ULL(63, 32); +#endif /* CONFIG_IIO_ST_LSM6DSOX_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_lsm6dsox_set_fifo_mode(struct st_lsm6dsox_hw *hw, + enum st_lsm6dsox_fifo_mode fifo_mode) +{ + int err; + + err = st_lsm6dsox_write_with_mask_locked(hw, + ST_LSM6DSOX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSOX_REG_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +static inline int +st_lsm6dsox_set_sensor_batching_odr(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + struct st_lsm6dsox_hw *hw = sensor->hw; + enum st_lsm6dsox_sensor_id id = sensor->id; + u8 data = 0; + int err; + + if (enable) { + err = st_lsm6dsox_get_batch_val(sensor, sensor->odr, + sensor->uodr, &data); + if (err < 0) + return err; + } + + return st_lsm6dsox_update_bits_locked(hw, + hw->st_lsm6dsox_odr_table[id].batching_reg.addr, + hw->st_lsm6dsox_odr_table[id].batching_reg.mask, + data); +} + +int st_lsm6dsox_update_watermark(struct st_lsm6dsox_sensor *sensor, + u16 watermark) +{ + u16 fifo_watermark = ST_LSM6DSOX_MAX_FIFO_DEPTH, cur_watermark = 0; + struct st_lsm6dsox_hw *hw = sensor->hw; + struct st_lsm6dsox_sensor *cur_sensor; + __le16 wdata; + int i, err; + int data = 0; + + for (i = ST_LSM6DSOX_ID_GYRO; i <= ST_LSM6DSOX_ID_STEP_COUNTER; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + + 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_LSM6DSOX_REG_FIFO_CTRL1_ADDR + 1, + &data); + if (err < 0) + goto out; + + fifo_watermark = ((data << 8) & ~ST_LSM6DSOX_REG_FIFO_WTM_MASK) | + (fifo_watermark & ST_LSM6DSOX_REG_FIFO_WTM_MASK); + wdata = cpu_to_le16(fifo_watermark); + + err = regmap_bulk_write(hw->regmap, ST_LSM6DSOX_REG_FIFO_CTRL1_ADDR, + &wdata, sizeof(wdata)); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +static struct +iio_dev *st_lsm6dsox_get_iiodev_from_tag(struct st_lsm6dsox_hw *hw, u8 tag) +{ + struct iio_dev *iio_dev; + + switch (tag) { + case ST_LSM6DSOX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_GYRO]; + break; + case ST_LSM6DSOX_ACC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_ACC]; + break; + case ST_LSM6DSOX_TEMP_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_TEMP]; + break; + case ST_LSM6DSOX_EXT0_TAG: + if (hw->enable_mask & BIT(ST_LSM6DSOX_ID_EXT0)) + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_EXT0]; + else + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_EXT1]; + break; + case ST_LSM6DSOX_EXT1_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_EXT1]; + break; + case ST_LSM6DSOX_SC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_STEP_COUNTER]; + break; + default: + iio_dev = NULL; + break; + } + + return iio_dev; +} + +static int st_lsm6dsox_read_fifo(struct st_lsm6dsox_hw *hw) +{ + u8 iio_buf[ALIGN(ST_LSM6DSOX_FIFO_SAMPLE_SIZE, sizeof(s64)) + + sizeof(s64) + sizeof(s64)]; + u8 buf[6 * ST_LSM6DSOX_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + struct st_lsm6dsox_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; + + /* return if FIFO is already disabled */ + if (hw->fifo_mode == ST_LSM6DSOX_FIFO_BYPASS) + return 0; + + ts_irq = hw->ts - hw->delta_ts; + + err = st_lsm6dsox_read_locked(hw, ST_LSM6DSOX_REG_FIFO_STATUS1_ADDR, + &fifo_status, sizeof(fifo_status)); + if (err < 0) + return err; + + fifo_depth = le16_to_cpu(fifo_status) & + ST_LSM6DSOX_REG_FIFO_STATUS_DIFF; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_LSM6DSOX_FIFO_SAMPLE_SIZE; + read_len = 0; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_lsm6dsox_read_locked(hw, + ST_LSM6DSOX_REG_FIFO_DATA_OUT_TAG_ADDR, + buf, word_len); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_LSM6DSOX_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_LSM6DSOX_TAG_SIZE]; + tag = buf[i] >> 3; + + if (tag == ST_LSM6DSOX_TS_TAG) { + val = get_unaligned_le32(ptr); + +#if defined(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSOX_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_LSM6DSOX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + + hw_ts_old = hw->hw_ts; + hw->hw_ts = val * hw->ts_delta_ns; + hw->ts_offset = st_lsm6dsox_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_LSM6DSOX_EWMA_LEVEL); + 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_lsm6dsox_get_iiodev_from_tag(hw, + tag); + if (!iio_dev) + continue; + + sensor = iio_priv(iio_dev); + + /* Skip samples if not ready */ + drdymask = (s16)le16_to_cpu(get_unaligned_le16(ptr)); + if (unlikely(drdymask >= + ST_LSM6DSOX_SAMPLE_DISCHARD)) { + continue; + } + + /* + * hw ts in not queued in FIFO if only step + * counter enabled + */ + if (sensor->id == ST_LSM6DSOX_ID_STEP_COUNTER) { + val = get_unaligned_le32(ptr + 2); + hw->tsample = val * hw->ts_delta_ns; + } else { + +#if defined(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + + hw_timestamp_push = cpu_to_le64(hw->hw_timestamp_global); + +#if defined(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + + memcpy(&iio_buf[ALIGN(ST_LSM6DSOX_SAMPLE_SIZE, sizeof(s64))], + &hw_timestamp_push, sizeof(hw_timestamp_push)); + hw->tsample = min_t(s64, + iio_get_time_ns(hw->iio_devs[0]), + hw->tsample); + sensor->last_fifo_timestamp = hw_timestamp_push; + } + + memcpy(iio_buf, ptr, ST_LSM6DSOX_SAMPLE_SIZE); + + /* support decimation for ODR < 12.5 Hz */ + if (sensor->dec_counter > 0) { + sensor->dec_counter--; + } else { + sensor->dec_counter = sensor->decimator; + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + } + } + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lsm6dsox_get_max_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +ssize_t st_lsm6dsox_get_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +ssize_t st_lsm6dsox_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_lsm6dsox_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_lsm6dsox_update_watermark(sensor, val); + if (err < 0) + goto out; + + sensor->watermark = val; + iio_device_release_direct_mode(iio_dev); + +out: + return err < 0 ? err : size; +} + +ssize_t st_lsm6dsox_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_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + s64 type; + s64 event; + int count; + s64 fts; + s64 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_LSM6DSOX_HW_FLUSH, &hw->state); + count = st_lsm6dsox_read_fifo(hw); + fts = sensor->last_fifo_timestamp; + sensor->dec_counter = 0; + mutex_unlock(&hw->fifo_lock); + + type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fts); + + return size; +} + +int st_lsm6dsox_suspend_fifo(struct st_lsm6dsox_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + st_lsm6dsox_read_fifo(hw); + err = st_lsm6dsox_set_fifo_mode(hw, ST_LSM6DSOX_FIFO_BYPASS); + mutex_unlock(&hw->fifo_lock); + + return err; +} + +int st_lsm6dsox_update_batching(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + err = st_lsm6dsox_set_sensor_batching_odr(sensor, enable); + enable_irq(hw->irq); + + return err; +} + +static int st_lsm6dsox_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + int err; + + if (sensor->id == ST_LSM6DSOX_ID_GYRO && !enable) + delayed_enable_gyro = true; + + if (sensor->id == ST_LSM6DSOX_ID_GYRO && + enable && delayed_enable_gyro) { + delayed_enable_gyro = false; + msleep(lsm6dsox_delay_gyro); + } + + disable_irq(hw->irq); + +#if defined(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) + hrtimer_cancel(&hw->timesync_timer); + cancel_work_sync(&hw->timesync_work); +#endif /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + + switch (sensor->id) { + case ST_LSM6DSOX_ID_EXT0: + case ST_LSM6DSOX_ID_EXT1: + err = st_lsm6dsox_shub_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSOX_ID_STEP_COUNTER: + err = st_lsm6dsox_step_counter_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSOX_ID_TEMP: + /* + * 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_LSM6DSOX_ID_ACC) | + BIT(ST_LSM6DSOX_ID_GYRO)))) { + struct st_lsm6dsox_sensor *acc_sensor; + u8 data = 0; + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSOX_ID_ACC]); + if (enable) { + err = st_lsm6dsox_get_batch_val(acc_sensor, + sensor->odr, + sensor->uodr, + &data); + if (err < 0) + goto out; + } + + err = st_lsm6dsox_update_bits_locked(hw, + hw->st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].batching_reg.addr, + hw->st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].batching_reg.mask, + data); + if (err < 0) + goto out; + } + break; + default: + err = st_lsm6dsox_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + + err = st_lsm6dsox_set_sensor_batching_odr(sensor, enable); + if (err < 0) + goto out; + break; + } + + err = st_lsm6dsox_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (enable && hw->fifo_mode == ST_LSM6DSOX_FIFO_BYPASS) { + st_lsm6dsox_reset_hwts(hw); + err = st_lsm6dsox_set_fifo_mode(hw, ST_LSM6DSOX_FIFO_CONT); + } else if (!hw->enable_mask) { + err = st_lsm6dsox_set_fifo_mode(hw, ST_LSM6DSOX_FIFO_BYPASS); + } + +#if defined(CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP) + if (hw->fifo_mode != ST_LSM6DSOX_FIFO_BYPASS) { + hrtimer_start(&hw->timesync_timer, + ktime_set(0, 0), + HRTIMER_MODE_REL); + } +#endif /* CONFIG_IIO_ST_LSM6DSOX_ASYNC_HW_TIMESTAMP */ + +out: + enable_irq(hw->irq); + + return err; +} + +static irqreturn_t st_lsm6dsox_handler_irq(int irq, void *private) +{ + struct st_lsm6dsox_hw *hw = (struct st_lsm6dsox_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_lsm6dsox_handler_thread(int irq, void *private) +{ + struct st_lsm6dsox_hw *hw = (struct st_lsm6dsox_hw *)private; + + if (st_lsm6dsox_run_mlc_task(hw)) + st_lsm6dsox_mlc_check_status(hw); + + mutex_lock(&hw->fifo_lock); + st_lsm6dsox_read_fifo(hw); + clear_bit(ST_LSM6DSOX_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + + if (hw->enable_mask & (BIT(ST_LSM6DSOX_ID_STEP_DETECTOR) | + BIT(ST_LSM6DSOX_ID_TILT) | + BIT(ST_LSM6DSOX_ID_SIGN_MOTION))) { + struct iio_dev *iio_dev; + u8 status[3]; + s64 event; + int err; + + err = regmap_bulk_read(hw->regmap, + ST_LSM6DSOX_REG_EMB_FUNC_STATUS_MAINPAGE, + status, sizeof(status)); + if (err < 0) + goto out; + + /* embedded function sensors */ + if (status[0] & ST_LSM6DSOX_REG_INT_STEP_DET_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_STEP_DETECTOR]; + event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status[0] & ST_LSM6DSOX_REG_INT_SIGMOT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_SIGN_MOTION]; + event = IIO_UNMOD_EVENT_CODE(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[0] & ST_LSM6DSOX_REG_INT_TILT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSOX_ID_TILT]; + event = IIO_UNMOD_EVENT_CODE(IIO_TILT, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + } + +out: + return IRQ_HANDLED; +} + +static int st_lsm6dsox_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsox_update_fifo(iio_dev, true); +} + +static int st_lsm6dsox_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsox_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsox_fifo_ops = { + .preenable = st_lsm6dsox_fifo_preenable, + .postdisable = st_lsm6dsox_fifo_postdisable, +}; + +int st_lsm6dsox_buffers_setup(struct st_lsm6dsox_hw *hw) +{ + struct device_node *np = hw->dev->of_node; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + 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 = regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_CTRL3_C_ADDR, + ST_LSM6DSOX_REG_H_LACTIVE_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_H_LACTIVE_MASK, + irq_active_low)); + if (err < 0) + return err; + + if (np && of_property_read_bool(np, "drive-open-drain")) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_CTRL3_C_ADDR, + ST_LSM6DSOX_REG_PP_OD_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_PP_OD_MASK, 1)); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lsm6dsox_handler_irq, + st_lsm6dsox_handler_thread, + irq_type | IRQF_ONESHOT, + hw->dev_name, hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return err; + } + + for (i = ST_LSM6DSOX_ID_GYRO; i <= ST_LSM6DSOX_ID_STEP_COUNTER; i++) { + if (!hw->iio_devs[i]) + continue; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + INDIO_BUFFER_SOFTWARE, + &st_lsm6dsox_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_lsm6dsox_fifo_ops; +#endif /* LINUX_VERSION_CODE */ + + } + + err = st_lsm6dsox_hwtimesync_init(hw); + if (err) + return err; + + return regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSOX_REG_DEC_TS_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_DEC_TS_MASK, 1)); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_core.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_core.c new file mode 100644 index 000000000000..f09e40bbe581 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_core.c @@ -0,0 +1,2307 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsox.h" + +static struct st_lsm6dsox_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_lsm6dsox_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DSOX_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_LSM6DSOX_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DSOX_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DSOX_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DSOX_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DSOX_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +static struct st_lsm6dsox_power_mode_table { + char *string_mode; + enum st_lsm6dsox_pm_t val; +} st_lsm6dsox_power_mode[] = { + [0] = { + .string_mode = "HP_MODE", + .val = ST_LSM6DSOX_HP_MODE, + }, + [1] = { + .string_mode = "LP_MODE", + .val = ST_LSM6DSOX_LP_MODE, + }, +}; + +static struct st_lsm6dsox_suspend_resume_entry + st_lsm6dsox_suspend_resume[ST_LSM6DSOX_SUSPEND_RESUME_REGS] = { + [ST_LSM6DSOX_CTRL1_XL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_LSM6DSOX_CTRL2_G_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_LSM6DSOX_REG_CTRL3_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_CTRL3_C_ADDR, + .mask = ST_LSM6DSOX_REG_BDU_MASK | + ST_LSM6DSOX_REG_PP_OD_MASK | + ST_LSM6DSOX_REG_H_LACTIVE_MASK, + }, + [ST_LSM6DSOX_REG_CTRL4_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_CTRL4_C_ADDR, + .mask = ST_LSM6DSOX_REG_DRDY_MASK, + }, + [ST_LSM6DSOX_REG_CTRL5_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_CTRL5_C_ADDR, + .mask = ST_LSM6DSOX_REG_ROUNDING_MASK, + }, + [ST_LSM6DSOX_REG_CTRL10_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_CTRL10_C_ADDR, + .mask = ST_LSM6DSOX_REG_TIMESTAMP_EN_MASK, + }, + [ST_LSM6DSOX_REG_TAP_CFG0_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_TAP_CFG0_ADDR, + .mask = ST_LSM6DSOX_REG_LIR_MASK, + }, + [ST_LSM6DSOX_REG_INT1_CTRL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_INT1_CTRL_ADDR, + .mask = ST_LSM6DSOX_REG_FIFO_TH_MASK, + }, + [ST_LSM6DSOX_REG_INT2_CTRL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_INT2_CTRL_ADDR, + .mask = ST_LSM6DSOX_REG_FIFO_TH_MASK, + }, + [ST_LSM6DSOX_REG_FIFO_CTRL1_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_FIFO_CTRL1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSOX_REG_FIFO_CTRL2_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_FIFO_CTRL2_ADDR, + .mask = ST_LSM6DSOX_REG_FIFO_WTM8_MASK, + }, + [ST_LSM6DSOX_REG_FIFO_CTRL3_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_FIFO_CTRL3_ADDR, + .mask = ST_LSM6DSOX_REG_BDR_XL_MASK | + ST_LSM6DSOX_REG_BDR_GY_MASK, + }, + [ST_LSM6DSOX_REG_FIFO_CTRL4_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSOX_REG_FIFO_CTRL4_ADDR, + .mask = ST_LSM6DSOX_REG_DEC_TS_MASK | + ST_LSM6DSOX_REG_ODR_T_BATCH_MASK, + }, + [ST_LSM6DSOX_REG_EMB_FUNC_EN_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_EMB_FUNC_EN_B_ADDR, + .mask = ST_LSM6DSOX_FSM_EN_MASK | + ST_LSM6DSOX_MLC_EN_MASK, + }, + [ST_LSM6DSOX_REG_FSM_INT1_A_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_FSM_INT1_A_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSOX_REG_FSM_INT1_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_FSM_INT1_B_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSOX_REG_MLC_INT1_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_MLC_INT1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSOX_REG_FSM_INT2_A_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_FSM_INT2_A_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSOX_REG_FSM_INT2_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_FSM_INT2_B_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSOX_REG_MLC_INT2_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSOX_MLC_INT2_ADDR, + .mask = GENMASK(7, 0), + }, +}; + +static const struct st_lsm6dsox_odr_table_entry st_lsm6dsox_odr_table[] = { + [ST_LSM6DSOX_ID_ACC] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSOX_CTRL1_XL_ADDR, + .mask = GENMASK(7, 4), + }, + .pm = { + .addr = ST_LSM6DSOX_REG_CTRL6_C_ADDR, + .mask = ST_LSM6DSOX_REG_XL_HM_MODE_MASK, + }, + .batching_reg = { + .addr = ST_LSM6DSOX_REG_FIFO_CTRL3_ADDR, + .mask = GENMASK(3, 0), + }, + .odr_avl[0] = { 1, 600000, 0x01, 0x0b }, + .odr_avl[1] = { 12, 500000, 0x01, 0x01 }, + .odr_avl[2] = { 26, 0, 0x02, 0x02 }, + .odr_avl[3] = { 52, 0, 0x03, 0x03 }, + .odr_avl[4] = { 104, 0, 0x04, 0x04 }, + .odr_avl[5] = { 208, 0, 0x05, 0x05 }, + .odr_avl[6] = { 416, 0, 0x06, 0x06 }, + .odr_avl[7] = { 833, 0, 0x07, 0x07 }, + }, + [ST_LSM6DSOX_ID_GYRO] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSOX_CTRL2_G_ADDR, + .mask = GENMASK(7, 4), + }, + .pm = { + .addr = ST_LSM6DSOX_REG_CTRL7_G_ADDR, + .mask = ST_LSM6DSOX_REG_G_HM_MODE_MASK, + }, + .batching_reg = { + .addr = ST_LSM6DSOX_REG_FIFO_CTRL3_ADDR, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 6, 500000, 0x01, 0x0b }, + .odr_avl[1] = { 12, 500000, 0x01, 0x01 }, + .odr_avl[2] = { 26, 0, 0x02, 0x02 }, + .odr_avl[3] = { 52, 0, 0x03, 0x03 }, + .odr_avl[4] = { 104, 0, 0x04, 0x04 }, + .odr_avl[5] = { 208, 0, 0x05, 0x05 }, + .odr_avl[6] = { 416, 0, 0x06, 0x06 }, + .odr_avl[7] = { 833, 0, 0x07, 0x07 }, + }, + [ST_LSM6DSOX_ID_TEMP] = { + .size = 3, + .batching_reg = { + .addr = ST_LSM6DSOX_REG_FIFO_CTRL4_ADDR, + .mask = GENMASK(5, 4), + }, + .odr_avl[0] = { 1, 600000, 0x01, 0x01 }, + .odr_avl[1] = { 12, 500000, 0x02, 0x02 }, + .odr_avl[2] = { 52, 0, 0x03, 0x03 }, + }, +}; + +/** + * List of supported Full Scale Values + * + * The following table is complete list of supported Full Scale by Acc, + * Gyro and Temp sensors. + */ +static const struct st_lsm6dsox_settings st_lsm6dsox_sensor_settings[] = { + { + .id = { + { + .hw_id = ST_LSM6DSO_ID, + .name = ST_LSM6DSO_DEV_NAME, + }, + }, + .st_fsm_probe = true, + .fs_table = { + [ST_LSM6DSOX_ID_ACC] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSOX_ID_GYRO] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + }, + { + .id = { + { + .hw_id = ST_LSM6DSOX_ID, + .name = ST_LSM6DSOX_DEV_NAME, + }, + }, + .st_mlc_probe = true, + .st_fsm_probe = true, + .fs_table = { + [ST_LSM6DSOX_ID_ACC] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSOX_ID_GYRO] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + }, + { + .id = { + { + .hw_id = ST_LSM6DSO32_ID, + .name = ST_LSM6DSO32_DEV_NAME, + }, + }, + .st_fsm_probe = true, + .fs_table = { + [ST_LSM6DSOX_ID_ACC] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(122000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(244000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(488000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(976000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSOX_ID_GYRO] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + }, + { + .id = { + { + .hw_id = ST_LSM6DSO32X_ID, + .name = ST_LSM6DSO32X_DEV_NAME, + }, + }, + .st_mlc_probe = true, + .st_fsm_probe = true, + .fs_table = { + [ST_LSM6DSOX_ID_ACC] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(122000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(244000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(488000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(976000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSOX_ID_GYRO] = { + .reg = { + .addr = ST_LSM6DSOX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + }, +}; + +static const struct iio_mount_matrix * +st_lsm6dsox_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *ch) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct iio_chan_spec_ext_info st_lsm6dsox_chan_spec_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, st_lsm6dsox_get_mount_matrix), + { } +}; + +#define IIO_CHAN_HW_TIMESTAMP(si) { \ + .type = IIO_COUNT, \ + .address = ST_LSM6DSOX_REG_TIMESTAMP0_ADDR, \ + .scan_index = si, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 64, \ + .storagebits = 64, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_lsm6dsox_acc_channels[] = { + ST_LSM6DSOX_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSOX_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsox_chan_spec_ext_info), + ST_LSM6DSOX_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSOX_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsox_chan_spec_ext_info), + ST_LSM6DSOX_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSOX_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsox_chan_spec_ext_info), + ST_LSM6DSOX_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsox_gyro_channels[] = { + ST_LSM6DSOX_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSOX_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsox_chan_spec_ext_info), + ST_LSM6DSOX_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSOX_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsox_chan_spec_ext_info), + ST_LSM6DSOX_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSOX_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsox_chan_spec_ext_info), + ST_LSM6DSOX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsox_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LSM6DSOX_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_LSM6DSOX_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_HW_TIMESTAMP(1), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + + +/** + * 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_lsm6dsox_step_counter_channels[] = { + { + .type = IIO_STEP_COUNTER, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + ST_LSM6DSOX_EVENT_CHANNEL(IIO_STEP_COUNTER, flush), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +/** + * @brief 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_lsm6dsox_step_detector_channels[] = { + ST_LSM6DSOX_EVENT_CHANNEL(IIO_STEP_DETECTOR, 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_lsm6dsox_sign_motion_channels[] = { + ST_LSM6DSOX_EVENT_CHANNEL(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_lsm6dsox_tilt_channels[] = { + ST_LSM6DSOX_EVENT_CHANNEL(IIO_TILT, thr), +}; + +static __maybe_unused int st_lsm6dsox_reg_access(struct iio_dev *iio_dev, + unsigned int reg, unsigned int writeval, + unsigned int *readval) +{ + struct st_lsm6dsox_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_lsm6dsox_set_page_0(struct st_lsm6dsox_hw *hw) +{ + return regmap_write(hw->regmap, + ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR, 0); +} + +/** + * Detect device ID + * + * Check the value of the Device ID if valid + * + * @param hw: ST IMU MEMS hw instance. + * @param id: ST IMU MEMS id index. + * @param name: Store ST IMU sensor name. + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dsox_check_whoami(struct st_lsm6dsox_hw *hw, int id, + const char **name) +{ + int err, i, j, data; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsox_sensor_settings); i++) { + for (j = 0; j < ST_LSM6DSOX_MAX_ID; j++) { + if (st_lsm6dsox_sensor_settings[i].id[j].name && + st_lsm6dsox_sensor_settings[i].id[j].hw_id == id) + break; + } + + if (j < ST_LSM6DSOX_MAX_ID) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsox_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_LSM6DSOX_REG_WHOAMI_ADDR, + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + + return err; + } + + if (data != ST_LSM6DSOX_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + + return -ENODEV; + } + + *name = st_lsm6dsox_sensor_settings[i].id[j].name; + hw->settings = &st_lsm6dsox_sensor_settings[i]; + hw->st_lsm6dsox_odr_table = st_lsm6dsox_odr_table; + + return 0; +} + +static int st_lsm6dsox_get_odr_calibration(struct st_lsm6dsox_hw *hw) +{ + int err; + int data; + s64 odr_calib; + + err = regmap_read(hw->regmap, ST_LSM6DSOX_INTERNAL_FREQ_FINE, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %d register\n", + ST_LSM6DSOX_INTERNAL_FREQ_FINE); + return err; + } + + odr_calib = ((s8)data * 37500) / 1000; + hw->ts_delta_ns = ST_LSM6DSOX_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_lsm6dsox_set_full_scale(struct st_lsm6dsox_sensor *sensor, + u32 gain) +{ + const struct st_lsm6dsox_fs_table_entry *fs_table; + enum st_lsm6dsox_sensor_id id = sensor->id; + struct st_lsm6dsox_hw *hw = sensor->hw; + int i, err; + u8 val; + + fs_table = &sensor->hw->settings->fs_table[id]; + + for (i = 0; i < fs_table->fs_len; i++) + if (fs_table->fs_avl[i].gain == gain) + break; + + if (i == fs_table->fs_len) + return -EINVAL; + + val = fs_table->fs_avl[i].val; + err = regmap_update_bits(hw->regmap, + fs_table->reg.addr, + fs_table->reg.mask, + ST_LSM6DSOX_SHIFT_VAL(val, fs_table->reg.mask)); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dsox_get_odr_val(enum st_lsm6dsox_sensor_id id, int odr, + int uodr, struct st_lsm6dsox_odr *oe) +{ + int req_odr = ST_LSM6DSOX_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < st_lsm6dsox_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSOX_ODR_EXPAND( + st_lsm6dsox_odr_table[id].odr_avl[i].hz, + st_lsm6dsox_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= req_odr) { + oe->hz = st_lsm6dsox_odr_table[id].odr_avl[i].hz; + oe->uhz = st_lsm6dsox_odr_table[id].odr_avl[i].uhz; + oe->val = st_lsm6dsox_odr_table[id].odr_avl[i].val; + + return 0; + } + } + + return -EINVAL; +} + +int st_lsm6dsox_get_batch_val(struct st_lsm6dsox_sensor *sensor, int odr, + int uodr, u8 *val) +{ + enum st_lsm6dsox_sensor_id id = sensor->id; + int req_odr = ST_LSM6DSOX_ODR_EXPAND(odr, uodr); + int i; + int sensor_odr; + + for (i = 0; i < st_lsm6dsox_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSOX_ODR_EXPAND( + st_lsm6dsox_odr_table[id].odr_avl[i].hz, + st_lsm6dsox_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= req_odr) + break; + } + + if (i == st_lsm6dsox_odr_table[id].size) + return -EINVAL; + + *val = st_lsm6dsox_odr_table[id].odr_avl[i].batch_val; + + return 0; +} + +static u16 st_lsm6dsox_check_odr_dependency(struct st_lsm6dsox_hw *hw, + int odr, int uodr, + enum st_lsm6dsox_sensor_id ref_id) +{ + struct st_lsm6dsox_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = odr > 0; + u16 ret; + + if (enable) { + /* uodr not used */ + if (hw->enable_mask & BIT(ref_id)) + ret = max_t(u16, ref->odr, odr); + else + ret = odr; + } else { + ret = (hw->enable_mask & BIT(ref_id)) ? ref->odr : 0; + } + + return ret; +} + +static int st_lsm6dsox_set_odr(struct st_lsm6dsox_sensor *sensor, int req_odr, + int req_uodr) +{ + enum st_lsm6dsox_sensor_id id = sensor->id; + struct st_lsm6dsox_hw *hw = sensor->hw; + struct st_lsm6dsox_odr oe = { 0 }; + + switch (id) { + case ST_LSM6DSOX_ID_EXT0: + case ST_LSM6DSOX_ID_EXT1: + case ST_LSM6DSOX_ID_TEMP: + case ST_LSM6DSOX_ID_STEP_COUNTER: + case ST_LSM6DSOX_ID_STEP_DETECTOR: + case ST_LSM6DSOX_ID_SIGN_MOTION: + case ST_LSM6DSOX_ID_TILT: + case ST_LSM6DSOX_ID_FSM_0: + case ST_LSM6DSOX_ID_FSM_1: + case ST_LSM6DSOX_ID_FSM_2: + case ST_LSM6DSOX_ID_FSM_3: + case ST_LSM6DSOX_ID_FSM_4: + case ST_LSM6DSOX_ID_FSM_5: + case ST_LSM6DSOX_ID_FSM_6: + case ST_LSM6DSOX_ID_FSM_7: + case ST_LSM6DSOX_ID_FSM_8: + case ST_LSM6DSOX_ID_FSM_9: + case ST_LSM6DSOX_ID_FSM_10: + case ST_LSM6DSOX_ID_FSM_11: + case ST_LSM6DSOX_ID_FSM_12: + case ST_LSM6DSOX_ID_FSM_13: + case ST_LSM6DSOX_ID_FSM_14: + case ST_LSM6DSOX_ID_FSM_15: + case ST_LSM6DSOX_ID_MLC_0: + case ST_LSM6DSOX_ID_MLC_1: + case ST_LSM6DSOX_ID_MLC_2: + case ST_LSM6DSOX_ID_MLC_3: + case ST_LSM6DSOX_ID_MLC_4: + case ST_LSM6DSOX_ID_MLC_5: + case ST_LSM6DSOX_ID_MLC_6: + case ST_LSM6DSOX_ID_MLC_7: + case ST_LSM6DSOX_ID_ACC: { + int odr; + int i; + + id = ST_LSM6DSOX_ID_ACC; + for (i = ST_LSM6DSOX_ID_ACC; i < ST_LSM6DSOX_ID_MAX; i++) { + if (!hw->iio_devs[i] || i == sensor->id) + continue; + + odr = st_lsm6dsox_check_odr_dependency(hw, req_odr, + req_uodr, i); + if (odr != req_odr) { + /* device already configured */ + return 0; + } + } + break; + } + default: + break; + } + + if (ST_LSM6DSOX_ODR_EXPAND(req_odr, req_uodr) > 0) { + int err; + + err = st_lsm6dsox_get_odr_val(id, req_odr, req_uodr, &oe); + if (err) + return err; + + /* check if sensor supports power mode setting */ + if (sensor->pm != ST_LSM6DSOX_NO_MODE) { + err = regmap_update_bits(hw->regmap, + st_lsm6dsox_odr_table[id].pm.addr, + st_lsm6dsox_odr_table[id].pm.mask, + ST_LSM6DSOX_SHIFT_VAL(sensor->pm, + st_lsm6dsox_odr_table[id].pm.mask)); + if (err < 0) + return err; + } + } + + return regmap_update_bits(hw->regmap, + st_lsm6dsox_odr_table[id].reg.addr, + st_lsm6dsox_odr_table[id].reg.mask, + ST_LSM6DSOX_SHIFT_VAL(oe.val, + st_lsm6dsox_odr_table[id].reg.mask)); +} + +int st_lsm6dsox_sensor_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsox_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; +} + +static int st_lsm6dsox_read_oneshot(struct st_lsm6dsox_sensor *sensor, + u8 addr, int *val) +{ + struct st_lsm6dsox_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + if (sensor->id == ST_LSM6DSOX_ID_TEMP) { + u8 status; + + err = st_lsm6dsox_read_locked(hw, + ST_LSM6DSOX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (err < 0) + return err; + + if (status & ST_LSM6DSOX_REG_STATUS_TDA) { + err = st_lsm6dsox_read_locked(hw, addr, + &data, sizeof(data)); + if (err < 0) + return err; + + sensor->old_data = data; + } else { + data = sensor->old_data; + } + } else { + err = st_lsm6dsox_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + /* + * - use big delay for data valid because of drdy mask enabled + * - uodr is neglected in this operation + */ + delay = 10000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsox_read_locked(hw, addr, + &data, sizeof(data)); + + st_lsm6dsox_sensor_set_enable(sensor, false); + if (err < 0) + return err; + } + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dsox_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsox_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_lsm6dsox_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + 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_LSM6DSOX_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; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lsm6dsox_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsox_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + struct st_lsm6dsox_odr oe = { 0 }; + + err = st_lsm6dsox_get_odr_val(sensor->id, val, val2, &oe); + if (!err) { + sensor->odr = oe.hz; + sensor->uodr = oe.uhz; + + /* + * 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_LSM6DSOX_ID_GYRO: + case ST_LSM6DSOX_ID_ACC: { + err = st_lsm6dsox_set_odr(sensor, + sensor->odr, + sensor->uodr); + if (err < 0) + break; + + st_lsm6dsox_update_batching(iio_dev, 1); + } + break; + default: + break; + } + } + } + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Read sensor event configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param type: Event Type. + * @param dir: Event Direction. + * @return 1 if Enabled, 0 Disabled + */ +static int st_lsm6dsox_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_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/** + * Write sensor event configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param type: Event Type. + * @param dir: Event Direction. + * @param state: New event state. + * @return 0 if OK, negative for ERROR + */ +static int st_lsm6dsox_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_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_lsm6dsox_embfunc_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_lsm6dsox_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsox_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dsox_odr_table[id].size; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lsm6dsox_odr_table[id].odr_avl[i].hz, + st_lsm6dsox_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsox_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + const struct st_lsm6dsox_fs_table_entry *fs_table; + enum st_lsm6dsox_sensor_id id = sensor->id; + int i, len = 0; + + fs_table = &sensor->hw->settings->fs_table[id]; + for (i = 0; i < fs_table->fs_len; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + fs_table->fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lsm6dsox_sysfs_get_power_mode_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsox_power_mode); i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%s ", + st_lsm6dsox_power_mode[i].string_mode); + } + + buf[len - 1] = '\n'; + + return len; +} + +ssize_t st_lsm6dsox_get_power_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%s\n", + st_lsm6dsox_power_mode[sensor->pm].string_mode); +} + +ssize_t st_lsm6dsox_set_power_mode(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + int err, i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsox_power_mode); i++) { + if (strncmp(buf, st_lsm6dsox_power_mode[i].string_mode, + strlen(st_lsm6dsox_power_mode[i].string_mode)) == 0) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsox_power_mode)) + return -EINVAL; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + /* update power mode */ + sensor->pm = st_lsm6dsox_power_mode[i].val; + + iio_device_release_direct_mode(iio_dev); + + return size; +} + +static int st_lsm6dsox_set_selftest( + struct st_lsm6dsox_sensor *sensor, int index) +{ + u8 mode, mask; + + switch (sensor->id) { + case ST_LSM6DSOX_ID_ACC: + mask = ST_LSM6DSOX_REG_ST_XL_MASK; + mode = st_lsm6dsox_selftest_table[index].accel_value; + break; + case ST_LSM6DSOX_ID_GYRO: + mask = ST_LSM6DSOX_REG_ST_G_MASK; + mode = st_lsm6dsox_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_lsm6dsox_update_bits_locked(sensor->hw, + ST_LSM6DSOX_REG_CTRL5_C_ADDR, + mask, mode); +} + +static ssize_t st_lsm6dsox_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6dsox_selftest_table[1].string_mode, + st_lsm6dsox_selftest_table[2].string_mode); +} + +static ssize_t st_lsm6dsox_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsox_sensor_id id = sensor->id; + + if (id != ST_LSM6DSOX_ID_ACC && + id != ST_LSM6DSOX_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_lsm6dsox_selftest_sensor(struct st_lsm6dsox_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_LSM6DSOX_ID_ACC: + reg = ST_LSM6DSOX_REG_OUTX_L_A_ADDR; + bitmask = ST_LSM6DSOX_REG_STATUS_XLDA; + break; + case ST_LSM6DSOX_ID_GYRO: + reg = ST_LSM6DSOX_REG_OUTX_L_G_ADDR; + bitmask = ST_LSM6DSOX_REG_STATUS_GDA; + break; + default: + return -EINVAL; + } + + /* set selftest normal mode */ + ret = st_lsm6dsox_set_selftest(sensor, 0); + if (ret < 0) + return ret; + + ret = st_lsm6dsox_sensor_set_enable(sensor, true); + if (ret < 0) + return ret; + + /* wait at least 2 ODRs to be sure */ + delay = 2 * (1000000 / 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, 2 * delay); + ret = st_lsm6dsox_read_locked(sensor->hw, + ST_LSM6DSOX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsox_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 acc samples missing (expected %d, read %d)\n", + i, n); + ret = -1; + + goto selftest_failure; + } + + n = 0; + + /* set selftest mode */ + st_lsm6dsox_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, 2 * delay); + ret = st_lsm6dsox_read_locked(sensor->hw, + ST_LSM6DSOX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsox_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_lsm6dsox_set_selftest(sensor, 0); + + return st_lsm6dsox_sensor_set_enable(sensor, false); +} + +int st_lsm6dsox_of_get_pin(struct st_lsm6dsox_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_lsm6dsox_get_int_reg(struct st_lsm6dsox_hw *hw, u8 *drdy_reg) +{ + int err = 0, int_pin; + + if (st_lsm6dsox_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: + hw->embfunc_pg0_irq_reg = ST_LSM6DSOX_REG_MD1_CFG_ADDR; + hw->embfunc_irq_reg = ST_LSM6DSOX_EMB_FUNC_INT1_ADDR; + *drdy_reg = ST_LSM6DSOX_REG_INT1_CTRL_ADDR; + break; + case 2: + hw->embfunc_pg0_irq_reg = ST_LSM6DSOX_REG_MD2_CFG_ADDR; + hw->embfunc_irq_reg = ST_LSM6DSOX_EMB_FUNC_INT2_ADDR; + *drdy_reg = ST_LSM6DSOX_REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +static int __maybe_unused st_lsm6dsox_bk_regs(struct st_lsm6dsox_hw *hw) +{ + unsigned int data; + bool restore = 0; + int i, err = 0; + + mutex_lock(&hw->page_lock); + + for (i = 0; i < ST_LSM6DSOX_SUSPEND_RESUME_REGS; i++) { + if (st_lsm6dsox_suspend_resume[i].page != FUNC_CFG_ACCESS_0) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSOX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_ACCESS_MASK, + st_lsm6dsox_suspend_resume[i].page)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsox_suspend_resume[i].addr); + break; + } + + restore = 1; + } + + err = regmap_read(hw->regmap, + st_lsm6dsox_suspend_resume[i].addr, + &data); + if (err < 0) { + dev_err(hw->dev, + "failed to save register %02x\n", + st_lsm6dsox_suspend_resume[i].addr); + goto out_lock; + } + + if (restore) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSOX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_ACCESS_MASK, + FUNC_CFG_ACCESS_0)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsox_suspend_resume[i].addr); + break; + } + + restore = 0; + } + + st_lsm6dsox_suspend_resume[i].val = data; + } + +out_lock: + mutex_unlock(&hw->page_lock); + + return err; +} + +static int __maybe_unused st_lsm6dsox_restore_regs(struct st_lsm6dsox_hw *hw) +{ + bool restore = 0; + int i, err = 0; + + mutex_lock(&hw->page_lock); + + for (i = 0; i < ST_LSM6DSOX_SUSPEND_RESUME_REGS; i++) { + if (st_lsm6dsox_suspend_resume[i].page != FUNC_CFG_ACCESS_0) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSOX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_ACCESS_MASK, + st_lsm6dsox_suspend_resume[i].page)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsox_suspend_resume[i].addr); + break; + } + + restore = 1; + } + + err = regmap_update_bits(hw->regmap, + st_lsm6dsox_suspend_resume[i].addr, + st_lsm6dsox_suspend_resume[i].mask, + st_lsm6dsox_suspend_resume[i].val); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsox_suspend_resume[i].addr); + break; + } + + if (restore) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSOX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSOX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_ACCESS_MASK, + FUNC_CFG_ACCESS_0)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsox_suspend_resume[i].addr); + break; + } + + restore = 0; + } + } + + mutex_unlock(&hw->page_lock); + + return err; +} + +static ssize_t st_lsm6dsox_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_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + enum st_lsm6dsox_sensor_id id = sensor->id; + struct st_lsm6dsox_hw *hw = sensor->hw; + int ret, test; + u8 drdy_reg; + u32 gain; + + if (id != ST_LSM6DSOX_ID_ACC && + id != ST_LSM6DSOX_ID_GYRO) + return -EINVAL; + + for (test = 0; test < ARRAY_SIZE(st_lsm6dsox_selftest_table); test++) { + if (strncmp(buf, st_lsm6dsox_selftest_table[test].string_mode, + strlen(st_lsm6dsox_selftest_table[test].string_mode)) == 0) + break; + } + + if (test == ARRAY_SIZE(st_lsm6dsox_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; + } + + st_lsm6dsox_bk_regs(hw); + + /* disable FIFO watermak interrupt */ + ret = st_lsm6dsox_get_int_reg(hw, &drdy_reg); + if (ret < 0) + goto restore_regs; + + ret = st_lsm6dsox_update_bits_locked(hw, drdy_reg, + ST_LSM6DSOX_REG_FIFO_TH_MASK, 0); + if (ret < 0) + goto restore_regs; + + gain = sensor->gain; + if (id == ST_LSM6DSOX_ID_ACC) { + /* set BDU = 1, FS = 4 g, ODR = 52 Hz */ + st_lsm6dsox_set_full_scale(sensor, IIO_G_TO_M_S_2(122)); + st_lsm6dsox_set_odr(sensor, 52, 0); + st_lsm6dsox_selftest_sensor(sensor, test); + + /* restore full scale after test */ + st_lsm6dsox_set_full_scale(sensor, gain); + } else { + /* set BDU = 1, ODR = 208 Hz, FS = 2000 dps */ + st_lsm6dsox_set_full_scale(sensor, IIO_DEGREE_TO_RAD(70000)); + st_lsm6dsox_set_odr(sensor, 208, 0); + st_lsm6dsox_selftest_sensor(sensor, test); + + /* restore full scale after test */ + st_lsm6dsox_set_full_scale(sensor, gain); + } + +restore_regs: + st_lsm6dsox_restore_regs(hw); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +/** + * Reset step counter value + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @param size: User buffer size. + * @return buffer len, negative for ERROR + */ +static ssize_t st_lsm6dsox_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 = st_lsm6dsox_reset_step_counter(iio_dev); + + return err < 0 ? err : size; +} + +ssize_t st_lsm6dsox_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static int st_lsm6dsox_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == 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 -EINVAL; + } + } + + return -EINVAL; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsox_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dsox_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dsox_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_lsm6dsox_sysfs_scale_avail, NULL, 0); + +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsox_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsox_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsox_get_watermark, + st_lsm6dsox_set_watermark, 0); + +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lsm6dsox_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_lsm6dsox_sysfs_get_selftest_status, + st_lsm6dsox_sysfs_start_selftest, 0); + +static IIO_DEVICE_ATTR(power_mode_available, 0444, + st_lsm6dsox_sysfs_get_power_mode_avail, NULL, 0); +static IIO_DEVICE_ATTR(power_mode, 0644, + st_lsm6dsox_get_power_mode, + st_lsm6dsox_set_power_mode, 0); +static IIO_DEVICE_ATTR(reset_counter, 0200, NULL, + st_lsm6dsox_sysfs_reset_step_counter, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsox_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsox_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.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_lsm6dsox_acc_attribute_group = { + .attrs = st_lsm6dsox_acc_attributes, +}; + +static const struct iio_info st_lsm6dsox_acc_info = { + .attrs = &st_lsm6dsox_acc_attribute_group, + .read_raw = st_lsm6dsox_read_raw, + .write_raw = st_lsm6dsox_write_raw, + .write_raw_get_fmt = st_lsm6dsox_write_raw_get_fmt, +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_lsm6dsox_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static struct attribute *st_lsm6dsox_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.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_lsm6dsox_gyro_attribute_group = { + .attrs = st_lsm6dsox_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsox_gyro_info = { + .attrs = &st_lsm6dsox_gyro_attribute_group, + .read_raw = st_lsm6dsox_read_raw, + .write_raw = st_lsm6dsox_write_raw, + .write_raw_get_fmt = st_lsm6dsox_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dsox_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_lsm6dsox_temp_attribute_group = { + .attrs = st_lsm6dsox_temp_attributes, +}; + +static const struct iio_info st_lsm6dsox_temp_info = { + .attrs = &st_lsm6dsox_temp_attribute_group, + .read_raw = st_lsm6dsox_read_raw, + .write_raw = st_lsm6dsox_write_raw, + .write_raw_get_fmt = st_lsm6dsox_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dsox_step_counter_attributes[] = { + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_reset_counter.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_lsm6dsox_step_counter_attribute_group = { + .attrs = st_lsm6dsox_step_counter_attributes, +}; + +static const struct iio_info st_lsm6dsox_step_counter_info = { + .attrs = &st_lsm6dsox_step_counter_attribute_group, +}; + +static struct attribute *st_lsm6dsox_step_detector_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsox_step_detector_attribute_group = { + .attrs = st_lsm6dsox_step_detector_attributes, +}; + +static const struct iio_info st_lsm6dsox_step_detector_info = { + .attrs = &st_lsm6dsox_step_detector_attribute_group, + .read_event_config = st_lsm6dsox_read_event_config, + .write_event_config = st_lsm6dsox_write_event_config, +}; + +static struct attribute *st_lsm6dsox_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsox_sign_motion_attribute_group = { + .attrs = st_lsm6dsox_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6dsox_sign_motion_info = { + .attrs = &st_lsm6dsox_sign_motion_attribute_group, + .read_event_config = st_lsm6dsox_read_event_config, + .write_event_config = st_lsm6dsox_write_event_config, +}; + +static struct attribute *st_lsm6dsox_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsox_tilt_attribute_group = { + .attrs = st_lsm6dsox_tilt_attributes, +}; + +static const struct iio_info st_lsm6dsox_tilt_info = { + .attrs = &st_lsm6dsox_tilt_attribute_group, + .read_event_config = st_lsm6dsox_read_event_config, + .write_event_config = st_lsm6dsox_write_event_config, +}; + +static const unsigned long st_lsm6dsox_available_scan_masks[] = { + GENMASK(3, 0), + 0x0 +}; +static const unsigned long st_lsm6dsox_temp_available_scan_masks[] = { + 0x1, 0x0 +}; +static const unsigned long st_lsm6dsox_emb_available_scan_masks[] = { + 0x1, 0x0 +}; + +static int st_lsm6dsox_reset_device(struct st_lsm6dsox_hw *hw) +{ + int err; + + /* sw reset */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_CTRL3_C_ADDR, + ST_LSM6DSOX_REG_SW_RESET_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_SW_RESET_MASK, 1)); + if (err < 0) + return err; + + msleep(50); + + /* boot */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_CTRL3_C_ADDR, + ST_LSM6DSOX_REG_BOOT_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_BOOT_MASK, 1)); + + msleep(50); + + return err; +} + +static int st_lsm6dsox_init_timestamp_engine(struct st_lsm6dsox_hw *hw, + bool enable) +{ + int err; + + /* Init timestamp engine. */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_CTRL10_C_ADDR, + ST_LSM6DSOX_REG_TIMESTAMP_EN_MASK, + ST_LSM6DSOX_SHIFT_VAL(true, + ST_LSM6DSOX_REG_TIMESTAMP_EN_MASK)); + if (err < 0) + return err; + + /* Enable timestamp rollover interrupt on INT2. */ + return regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_MD2_CFG_ADDR, + ST_LSM6DSOX_REG_INT2_TIMESTAMP_MASK, + ST_LSM6DSOX_SHIFT_VAL(enable, + ST_LSM6DSOX_REG_INT2_TIMESTAMP_MASK)); +} + +static int st_lsm6dsox_init_device(struct st_lsm6dsox_hw *hw) +{ + u8 drdy_reg; + int err; + + /* latch interrupts */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_TAP_CFG0_ADDR, + ST_LSM6DSOX_REG_LIR_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_LIR_MASK, 1)); + if (err < 0) + return err; + + /* enable Block Data Update */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_CTRL3_C_ADDR, + ST_LSM6DSOX_REG_BDU_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_BDU_MASK, 1)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_CTRL5_C_ADDR, + ST_LSM6DSOX_REG_ROUNDING_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_ROUNDING_MASK, 3)); + if (err < 0) + return err; + + err = st_lsm6dsox_init_timestamp_engine(hw, true); + if (err < 0) + return err; + + err = st_lsm6dsox_get_int_reg(hw, &drdy_reg); + if (err < 0) + return err; + + /* Enable DRDY MASK for filters settling time */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSOX_REG_CTRL4_C_ADDR, + ST_LSM6DSOX_REG_DRDY_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_DRDY_MASK, 1)); + + if (err < 0) + return err; + + /* enable enbedded function interrupts enable */ + err = regmap_update_bits(hw->regmap, hw->embfunc_pg0_irq_reg, + ST_LSM6DSOX_REG_INT_EMB_FUNC_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_INT_EMB_FUNC_MASK, + 1)); + if (err < 0) + return err; + + /* enable FIFO watermak interrupt */ + return regmap_update_bits(hw->regmap, drdy_reg, + ST_LSM6DSOX_REG_FIFO_TH_MASK, + FIELD_PREP(ST_LSM6DSOX_REG_FIFO_TH_MASK, 1)); +} + +static struct iio_dev *st_lsm6dsox_alloc_iiodev(struct st_lsm6dsox_hw *hw, + enum st_lsm6dsox_sensor_id id) +{ + struct st_lsm6dsox_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_LSM6DSOX_ID_ACC: + iio_dev->channels = st_lsm6dsox_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), "%s_accel", + hw->dev_name); + iio_dev->info = &st_lsm6dsox_acc_info; + iio_dev->available_scan_masks = + st_lsm6dsox_available_scan_masks; + sensor->max_watermark = ST_LSM6DSOX_MAX_FIFO_DEPTH; + st_lsm6dsox_set_full_scale(sensor, + sensor->hw->settings->fs_table[id].fs_avl[0].gain); + sensor->offset = 0; + sensor->pm = ST_LSM6DSOX_HP_MODE; + sensor->odr = st_lsm6dsox_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsox_odr_table[id].odr_avl[1].uhz; + sensor->min_st = ST_LSM6DSOX_SELFTEST_ACCEL_MIN; + sensor->max_st = ST_LSM6DSOX_SELFTEST_ACCEL_MAX; + break; + case ST_LSM6DSOX_ID_GYRO: + iio_dev->channels = st_lsm6dsox_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_gyro_channels); + scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro", + hw->dev_name); + iio_dev->info = &st_lsm6dsox_gyro_info; + iio_dev->available_scan_masks = + st_lsm6dsox_available_scan_masks; + sensor->max_watermark = ST_LSM6DSOX_MAX_FIFO_DEPTH; + st_lsm6dsox_set_full_scale(sensor, + sensor->hw->settings->fs_table[id].fs_avl[0].gain); + sensor->offset = 0; + sensor->pm = ST_LSM6DSOX_HP_MODE; + sensor->odr = st_lsm6dsox_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsox_odr_table[id].odr_avl[1].uhz; + sensor->min_st = ST_LSM6DSOX_SELFTEST_GYRO_MIN; + sensor->max_st = ST_LSM6DSOX_SELFTEST_GYRO_MAX; + break; + case ST_LSM6DSOX_ID_TEMP: + iio_dev->channels = st_lsm6dsox_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), "%s_temp", + hw->dev_name); + iio_dev->info = &st_lsm6dsox_temp_info; + iio_dev->available_scan_masks = + st_lsm6dsox_temp_available_scan_masks; + sensor->max_watermark = ST_LSM6DSOX_MAX_FIFO_DEPTH; + sensor->offset = ST_LSM6DSOX_TEMP_OFFSET; + sensor->pm = ST_LSM6DSOX_NO_MODE; + sensor->odr = st_lsm6dsox_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsox_odr_table[id].odr_avl[1].uhz; + break; + case ST_LSM6DSOX_ID_STEP_COUNTER: + iio_dev->channels = st_lsm6dsox_step_counter_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lsm6dsox_step_counter_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_step_c", hw->dev_name); + iio_dev->info = &st_lsm6dsox_step_counter_info; + iio_dev->available_scan_masks = + st_lsm6dsox_emb_available_scan_masks; + + /* request an acc ODR at least of 26 Hz to works properly */ + sensor->max_watermark = 1; + sensor->odr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].uhz; + break; + case ST_LSM6DSOX_ID_STEP_DETECTOR: + iio_dev->channels = st_lsm6dsox_step_detector_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lsm6dsox_step_detector_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_step_d", hw->dev_name); + iio_dev->info = &st_lsm6dsox_step_detector_info; + iio_dev->available_scan_masks = + st_lsm6dsox_emb_available_scan_masks; + + sensor->odr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].uhz; + break; + case ST_LSM6DSOX_ID_SIGN_MOTION: + iio_dev->channels = st_lsm6dsox_sign_motion_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lsm6dsox_sign_motion_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sign_motion", hw->dev_name); + iio_dev->info = &st_lsm6dsox_sign_motion_info; + iio_dev->available_scan_masks = + st_lsm6dsox_emb_available_scan_masks; + + sensor->odr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].uhz; + break; + case ST_LSM6DSOX_ID_TILT: + iio_dev->channels = st_lsm6dsox_tilt_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_tilt_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tilt", hw->dev_name); + iio_dev->info = &st_lsm6dsox_tilt_info; + iio_dev->available_scan_masks = + st_lsm6dsox_emb_available_scan_masks; + + sensor->odr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].hz; + sensor->uodr = + st_lsm6dsox_odr_table[ST_LSM6DSOX_ID_ACC].odr_avl[2].uhz; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static void st_lsm6dsox_disable_regulator_action(void *_data) +{ + struct st_lsm6dsox_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static void st_lsm6dsox_get_properties(struct st_lsm6dsox_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +static int st_lsm6dsox_power_enable(struct st_lsm6dsox_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_lsm6dsox_disable_regulator_action, + hw); + if (err) { + dev_err(hw->dev, + "Failed to setup regulator cleanup action %d\n", + err); + return err; + } + + return 0; +} + +/** + * Probe device function + * Implements [MODULE] feature for Power Management + * + * @param dev: Device pointer. + * @param irq: I2C/SPI/I3C client irq. + * @param hw_id: Sensor HW id. + * @param regmap: Bus Transfer Function pointer. + * @retval 0 if OK, < 0 for error + */ +int st_lsm6dsox_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap) +{ + struct st_lsm6dsox_hw *hw; + const char *name = NULL; + int i, err; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->page_lock); + + hw->regmap = regmap; + hw->dev = dev; + hw->irq = irq; + + err = st_lsm6dsox_power_enable(hw); + if (err != 0) + return err; + + err = st_lsm6dsox_set_page_0(hw); + if (err < 0) + return err; + + err = st_lsm6dsox_check_whoami(hw, hw_id, &name); + if (err < 0) + return err; + + scnprintf(hw->dev_name, sizeof(hw->dev_name), "%s", name); + + st_lsm6dsox_get_properties(hw); + + err = st_lsm6dsox_get_odr_calibration(hw); + if (err < 0) + return err; + + err = st_lsm6dsox_reset_device(hw); + if (err < 0) + return err; + + err = st_lsm6dsox_init_device(hw); + if (err < 0) + return err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,15,0) + err = iio_read_mount_matrix(hw->dev, &hw->orientation); +#elif LINUX_VERSION_CODE >= KERNEL_VERSION(5,2,0) + err = iio_read_mount_matrix(hw->dev, "mount-matrix", &hw->orientation); +#else /* LINUX_VERSION_CODE */ + err = of_iio_read_mount_matrix(hw->dev, "mount-matrix", &hw->orientation); +#endif /* LINUX_VERSION_CODE */ + + if (err) { + dev_err(dev, "Failed to retrieve mounting matrix %d\n", err); + return err; + } + + /* register only data sensors */ + for (i = 0; i < ARRAY_SIZE(st_lsm6dsox_main_sensor_list); i++) { + enum st_lsm6dsox_sensor_id id = st_lsm6dsox_main_sensor_list[i]; + + hw->iio_devs[id] = st_lsm6dsox_alloc_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + err = st_lsm6dsox_shub_probe(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_lsm6dsox_buffers_setup(hw); + if (err < 0) + return err; + } + + if (st_lsm6dsox_run_mlc_task(hw)) { + err = st_lsm6dsox_mlc_probe(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LSM6DSOX_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_lsm6dsox_run_mlc_task(hw)) { + err = st_lsm6dsox_mlc_init_preload(hw); + if (err) + return err; + } + + err = st_lsm6dsox_embedded_function_init(hw); + if (err) + return err; + + device_init_wakeup(dev, + device_property_read_bool(dev, "wakeup-source")); + + dev_info(dev, "Device probed\n"); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsox_probe); + +static int __maybe_unused st_lsm6dsox_suspend(struct device *dev) +{ + struct st_lsm6dsox_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSOX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + if (device_may_wakeup(dev) && + ((hw->enable_mask & BIT_ULL(sensor->id)) & + ST_LSM6DSOX_WAKE_UP_SENSORS)) { + /* do not disable sensors if requested by wake-up */ + err = st_lsm6dsox_set_odr(sensor, + ST_LSM6DSOX_MIN_ODR_IN_WAKEUP, + 0); + if (err < 0) + return err; + } else { + err = st_lsm6dsox_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + } + + if (st_lsm6dsox_is_fifo_enabled(hw)) { + err = st_lsm6dsox_suspend_fifo(hw); + if (err < 0) + return err; + } + + err = st_lsm6dsox_bk_regs(hw); + + if (device_may_wakeup(dev) && + (hw->enable_mask & ST_LSM6DSOX_WAKE_UP_SENSORS)) + enable_irq_wake(hw->irq); + + dev_info(dev, "Suspending device\n"); + + return err < 0 ? err : 0; +} + +static int __maybe_unused st_lsm6dsox_resume(struct device *dev) +{ + struct st_lsm6dsox_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsox_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Resuming device\n"); + + if (device_may_wakeup(dev)) + disable_irq_wake(hw->irq); + + err = st_lsm6dsox_restore_regs(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSOX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + if (hw->enable_mask & BIT_ULL(sensor->id)) { + err = st_lsm6dsox_set_odr(sensor, sensor->odr, + sensor->uodr); + if (err < 0) + return err; + } + } + + if (st_lsm6dsox_is_fifo_enabled(hw)) + err = st_lsm6dsox_set_fifo_mode(hw, ST_LSM6DSOX_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_lsm6dsox_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsox_suspend, st_lsm6dsox_resume) +}; +EXPORT_SYMBOL(st_lsm6dsox_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsox driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_embfunc.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_embfunc.c new file mode 100644 index 000000000000..7e06f40614c4 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_embfunc.c @@ -0,0 +1,219 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox embedded function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include + +#include "st_lsm6dsox.h" + +static int +st_lsm6dsox_ef_pg1_sensor_set_enable(struct st_lsm6dsox_sensor *sensor, + u8 mask, u8 irq_mask, bool enable) +{ + struct st_lsm6dsox_hw *hw = sensor->hw; + int err; + + err = st_lsm6dsox_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock; + + err = __st_lsm6dsox_write_with_mask(hw, + ST_LSM6DSOX_EMB_FUNC_EN_A_ADDR, + mask, enable); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsox_write_with_mask(hw, hw->embfunc_irq_reg, irq_mask, + enable); + +reset_page: + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable Embedded Function sensor [EMB_FUN] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor + * @return < 0 if error, 0 otherwise + */ +int st_lsm6dsox_embfunc_sensor_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + int err; + + switch (sensor->id) { + case ST_LSM6DSOX_ID_STEP_DETECTOR: + err = st_lsm6dsox_ef_pg1_sensor_set_enable(sensor, + ST_LSM6DSOX_PEDO_EN_MASK, + ST_LSM6DSOX_INT_STEP_DET_MASK, + enable); + break; + case ST_LSM6DSOX_ID_SIGN_MOTION: + err = st_lsm6dsox_ef_pg1_sensor_set_enable(sensor, + ST_LSM6DSOX_SIGN_MOTION_EN_MASK, + ST_LSM6DSOX_INT_SIGMOT_MASK, + enable); + break; + case ST_LSM6DSOX_ID_TILT: + err = st_lsm6dsox_ef_pg1_sensor_set_enable(sensor, + ST_LSM6DSOX_TILT_EN_MASK, + ST_LSM6DSOX_INT_TILT_MASK, + enable); + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +/** + * Enable Step Counter Sensor [EMB_FUN] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor + * @return < 0 if error, 0 otherwise + */ +int st_lsm6dsox_step_counter_set_enable(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + struct st_lsm6dsox_hw *hw = sensor->hw; + int err; + + err = st_lsm6dsox_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock; + + err = __st_lsm6dsox_write_with_mask(hw, + ST_LSM6DSOX_EMB_FUNC_EN_A_ADDR, + ST_LSM6DSOX_PEDO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsox_write_with_mask(hw, + ST_LSM6DSOX_EMB_FUNC_FIFO_CFG_ADDR, + ST_LSM6DSOX_PEDO_FIFO_EN_MASK, + enable); + +reset_page: + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Reset Step Counter value [EMB_FUN] + * + * @param iio_dev: IIO device + * @return < 0 if error, 0 otherwise + */ +int st_lsm6dsox_reset_step_counter(struct iio_dev *iio_dev) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + u16 prev_val, val = 0; + __le16 data; + int err; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock_iio_dev; + } + + err = st_lsm6dsox_step_counter_set_enable(sensor, true); + if (err < 0) + goto unlock_iio_dev; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock_page; + + do { + prev_val = val; + err = __st_lsm6dsox_write_with_mask(hw, + ST_LSM6DSOX_REG_EMB_FUNC_SRC_ADDR, + ST_LSM6DSOX_REG_PEDO_RST_STEP_MASK, 1); + if (err < 0) + goto reset_page; + + msleep(100); + + err = regmap_bulk_read(hw->regmap, + ST_LSM6DSOX_REG_STEP_COUNTER_L_ADDR, + (u8 *)&data, sizeof(data)); + if (err < 0) + goto reset_page; + + val = le16_to_cpu(data); + } while (val && val >= prev_val); + +reset_page: + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); +unlock_page: + mutex_unlock(&hw->page_lock); + + err = st_lsm6dsox_step_counter_set_enable(sensor, false); +unlock_iio_dev: + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Initialize Embedded funcrtion HW block [EMB_FUN] + * + * @param hw: ST IMU MEMS hw instance + * @return < 0 if error, 0 otherwise + */ +int st_lsm6dsox_embedded_function_init(struct st_lsm6dsox_hw *hw) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock; + + /* enable latched interrupts */ + err = __st_lsm6dsox_write_with_mask(hw, ST_LSM6DSOX_PAGE_RW_ADDR, + ST_LSM6DSOX_REG_EMB_FUNC_LIR_MASK, + 1); + + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_hwtimestamp.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_hwtimestamp.c new file mode 100644 index 000000000000..d580f8ef2efe --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_hwtimestamp.c @@ -0,0 +1,116 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox hwtimestamp library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsox.h" + +#define ST_LSM6DSOX_TSYNC_OFFSET_NS (300 * 1000LL) + +static void st_lsm6dsox_read_hw_timestamp(struct st_lsm6dsox_hw *hw) +{ + s64 timestamp_hw_global; + s64 eventLSB, eventMSB; + __le32 timestamp_hw; + s64 timestamp_cpu; + __le32 tmp; + int err; + + err = st_lsm6dsox_read_locked(hw, ST_LSM6DSOX_REG_TIMESTAMP0_ADDR, + (u8 *)×tamp_hw, + sizeof(timestamp_hw)); + if (err < 0) + return; + + timestamp_cpu = iio_get_time_ns(hw->iio_devs[0]) - + ST_LSM6DSOX_TSYNC_OFFSET_NS; + + eventLSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 0, + IIO_EV_TYPE_TIME_SYNC, 0, 0, 0); + eventMSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 1, + 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_LSM6DSOX_ID_GYRO)) { + iio_push_event(hw->iio_devs[ST_LSM6DSOX_ID_GYRO], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSOX_ID_GYRO], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSOX_ID_ACC)) { + iio_push_event(hw->iio_devs[ST_LSM6DSOX_ID_ACC], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSOX_ID_ACC], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSOX_ID_TEMP)) { + iio_push_event(hw->iio_devs[ST_LSM6DSOX_ID_TEMP], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSOX_ID_TEMP], eventMSB, + timestamp_cpu); + } + + if (hw->timesync_c < 6) + hw->timesync_c++; + else + hw->timesync_ktime = ktime_set(0, ST_LSM6DSOX_DEFAULT_KTIME); +} + +static void st_lsm6dsox_timesync_fn(struct work_struct *work) +{ + struct st_lsm6dsox_hw *hw = container_of(work, struct st_lsm6dsox_hw, + timesync_work); + + st_lsm6dsox_read_hw_timestamp(hw); +} + +static enum hrtimer_restart st_lsm6dsox_timer_fn(struct hrtimer *timer) +{ + struct st_lsm6dsox_hw *hw; + + hw = container_of(timer, struct st_lsm6dsox_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_lsm6dsox_hwtimesync_init(struct st_lsm6dsox_hw *hw) +{ + hw->timesync_c = 0; + hw->timesync_ktime = ktime_set(0, ST_LSM6DSOX_DEFAULT_KTIME); + hrtimer_init(&hw->timesync_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + hw->timesync_timer.function = st_lsm6dsox_timer_fn; + + spin_lock_init(&hw->hwtimestamp_lock); + hw->hw_timestamp_global = 0; + + hw->timesync_workqueue = create_singlethread_workqueue("st_lsm6dsox_workqueue"); + if (!hw->timesync_workqueue) + return -ENOMEM; + + INIT_WORK(&hw->timesync_work, st_lsm6dsox_timesync_fn); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsox_hwtimesync_init); diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i2c.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i2c.c new file mode 100644 index 000000000000..b75c77c85c9c --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i2c.c @@ -0,0 +1,95 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsox.h" + +static const struct regmap_config st_lsm6dsox_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsox_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_lsm6dsox_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_lsm6dsox_probe(&client->dev, client->irq, + hw_id, regmap); +} + +static int st_lsm6dsox_i2c_remove(struct i2c_client *client) +{ + int err = 0; + struct st_lsm6dsox_hw *hw = dev_get_drvdata(&client->dev); + + if (hw->settings->st_mlc_probe) + err = st_lsm6dsox_mlc_remove(&client->dev); + + return err; +} + +static const struct of_device_id st_lsm6dsox_i2c_of_match[] = { + { + .compatible = "st,lsm6dso", + .data = (void *)ST_LSM6DSO_ID, + }, + { + .compatible = "st,lsm6dsox", + .data = (void *)ST_LSM6DSOX_ID, + }, + { + .compatible = "st,lsm6dso32", + .data = (void *)ST_LSM6DSO32_ID, + }, + { + .compatible = "st,lsm6dso32x", + .data = (void *)ST_LSM6DSO32X_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsox_i2c_of_match); + +static const struct i2c_device_id st_lsm6dsox_i2c_id_table[] = { + { ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID }, + { ST_LSM6DSOX_DEV_NAME, ST_LSM6DSOX_ID }, + { ST_LSM6DSO32_DEV_NAME, ST_LSM6DSO32_ID }, + { ST_LSM6DSO32X_DEV_NAME, ST_LSM6DSO32X_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsox_i2c_id_table); + +static struct i2c_driver st_lsm6dsox_driver = { + .driver = { + .name = "st_" ST_LSM6DSOX_DEV_NAME "_i2c", + .pm = &st_lsm6dsox_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsox_i2c_of_match), + }, + .probe = st_lsm6dsox_i2c_probe, + .remove = st_lsm6dsox_i2c_remove, + .id_table = st_lsm6dsox_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsox_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsox i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i3c.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i3c.c new file mode 100644 index 000000000000..c6e97b6bbdc3 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_i3c.c @@ -0,0 +1,59 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox i3c driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsox.h" + +static const struct i3c_device_id st_lsm6dsox_i3c_ids[] = { + I3C_DEVICE(0x0104, ST_LSM6DSOX_WHOAMI_VAL, (void *)ST_LSM6DSO_ID), + {}, +}; +MODULE_DEVICE_TABLE(i3c, st_lsm6dsox_i3c_ids); + +static int st_lsm6dsox_i3c_probe(struct i3c_device *i3cdev) +{ + struct regmap_config st_lsm6dsox_i3c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + }; + const struct i3c_device_id *id = + i3c_device_match_id(i3cdev, st_lsm6dsox_i3c_ids); + struct regmap *regmap; + + regmap = devm_regmap_init_i3c(i3cdev, &st_lsm6dsox_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_lsm6dsox_probe(&i3cdev->dev, 0, + (uintptr_t)id->data, regmap); +} + +static struct i3c_driver st_lsm6dsox_driver = { + .driver = { + .name = "st_" ST_LSM6DSOX_DEV_NAME "_i3c", + .pm = &st_lsm6dsox_pm_ops, + }, + .probe = st_lsm6dsox_i3c_probe, + .id_table = st_lsm6dsox_i3c_ids, +}; +module_i3c_driver(st_lsm6dsox_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsox i3c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_mlc.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_mlc.c new file mode 100644 index 000000000000..908e6176911a --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_mlc.c @@ -0,0 +1,906 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox machine learning core driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsox.h" + +#define ST_LSM6DSOX_MLC_LOADER_VERSION "0.3" + +/* number of machine learning core available on device hardware */ +#define ST_LSM6DSOX_MLC_MAX_NUMBER 8 +#define ST_LSM6DSOX_FSM_MAX_NUMBER 16 + +#ifdef CONFIG_IIO_LSM6DSOX_MLC_BUILTIN_FIRMWARE +static const u8 st_lsm6dsox_mlc_fw[] = { + #include "st_lsm6dsox_mlc.fw" +}; +DECLARE_BUILTIN_FIRMWARE(LSM6DSOX_MLC_FIRMWARE_NAME, st_lsm6dsox_mlc_fw); +#else /* CONFIG_IIO_LSM6DSOX_MLC_BUILTIN_FIRMWARE */ +#define LSM6DSOX_MLC_FIRMWARE_NAME "st_lsm6dsox_mlc.bin" +#endif /* CONFIG_IIO_LSM6DSOX_MLC_BUILTIN_FIRMWARE */ + +static const u8 mlcdata[] = { + /* + * Machine Learning Core Tool v1.2.0.0 Beta, LSM6DSOX + * 6D position recognition + * source: + * https://github.com/STMicroelectronics/STMems_Machine_Learning_Core/blob/master/application_examples/lsm6dsox/6D%20position%20recognition/lsm6dsox_six_d_position.ucf + */ + 0x10, 0x00, 0x11, 0x00, 0x01, 0x80, 0x05, 0x00, 0x17, 0x40, + 0x02, 0x11, 0x08, 0xEA, 0x09, 0x58, 0x02, 0x11, 0x08, 0xEB, + 0x09, 0x03, 0x02, 0x11, 0x08, 0xEC, 0x09, 0x62, 0x02, 0x11, + 0x08, 0xED, 0x09, 0x03, 0x02, 0x11, 0x08, 0xEE, 0x09, 0x00, + 0x02, 0x11, 0x08, 0xEF, 0x09, 0x00, 0x02, 0x11, 0x08, 0xF0, + 0x09, 0x0A, 0x02, 0x11, 0x08, 0xF2, 0x09, 0x10, 0x02, 0x11, + 0x08, 0xFA, 0x09, 0x3C, 0x02, 0x11, 0x08, 0xFB, 0x09, 0x03, + 0x02, 0x11, 0x08, 0xFC, 0x09, 0x6E, 0x02, 0x11, 0x08, 0xFD, + 0x09, 0x03, 0x02, 0x11, 0x08, 0xFE, 0x09, 0x7A, 0x02, 0x11, + 0x08, 0xFF, 0x09, 0x03, 0x02, 0x31, 0x08, 0x3C, 0x09, 0x3F, + 0x02, 0x31, 0x08, 0x3D, 0x09, 0x00, 0x02, 0x31, 0x08, 0x3E, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x3F, 0x09, 0x84, 0x02, 0x31, + 0x08, 0x40, 0x09, 0x00, 0x02, 0x31, 0x08, 0x41, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x42, 0x09, 0x00, 0x02, 0x31, 0x08, 0x43, + 0x09, 0x88, 0x02, 0x31, 0x08, 0x44, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x45, 0x09, 0x00, 0x02, 0x31, 0x08, 0x46, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x47, 0x09, 0x8C, 0x02, 0x31, 0x08, 0x48, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x49, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x4A, 0x09, 0x00, 0x02, 0x31, 0x08, 0x4B, 0x09, 0x04, + 0x02, 0x31, 0x08, 0x4C, 0x09, 0x00, 0x02, 0x31, 0x08, 0x4D, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x4E, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x4F, 0x09, 0x08, 0x02, 0x31, 0x08, 0x50, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x51, 0x09, 0x00, 0x02, 0x31, 0x08, 0x52, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x53, 0x09, 0x0C, 0x02, 0x31, + 0x08, 0x54, 0x09, 0x00, 0x02, 0x31, 0x08, 0x55, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x56, 0x09, 0x1F, 0x02, 0x31, 0x08, 0x57, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x6E, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x6F, 0x09, 0x00, 0x02, 0x31, 0x08, 0x70, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x71, 0x09, 0x00, 0x02, 0x31, 0x08, 0x72, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x73, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x74, 0x09, 0x00, 0x02, 0x31, 0x08, 0x75, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x76, 0x09, 0x00, 0x02, 0x31, 0x08, 0x77, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x78, 0x09, 0x00, 0x01, 0x00, + 0x12, 0x00, 0x01, 0x80, 0x17, 0x40, 0x02, 0x31, 0x08, 0x7A, + 0x09, 0xCD, 0x02, 0x31, 0x08, 0x7B, 0x09, 0x34, 0x02, 0x31, + 0x08, 0x7C, 0x09, 0x05, 0x02, 0x31, 0x08, 0x7D, 0x09, 0x80, + 0x09, 0x00, 0x09, 0x00, 0x02, 0x31, 0x08, 0x7E, 0x09, 0xCD, + 0x02, 0x31, 0x08, 0x7F, 0x09, 0x34, 0x02, 0x31, 0x08, 0x80, + 0x09, 0x03, 0x02, 0x31, 0x08, 0x81, 0x09, 0x81, 0x09, 0x00, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x82, 0x09, 0xCD, 0x02, 0x31, + 0x08, 0x83, 0x09, 0x34, 0x02, 0x31, 0x08, 0x84, 0x09, 0x56, + 0x02, 0x31, 0x08, 0x85, 0x09, 0xE5, 0x09, 0x00, 0x09, 0x00, + 0x02, 0x31, 0x08, 0x86, 0x09, 0xCD, 0x02, 0x31, 0x08, 0x87, + 0x09, 0x34, 0x02, 0x31, 0x08, 0x88, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x89, 0x09, 0xA2, 0x09, 0x00, 0x09, 0x00, 0x02, 0x31, + 0x08, 0x8A, 0x09, 0xCD, 0x02, 0x31, 0x08, 0x8B, 0x09, 0x34, + 0x02, 0x31, 0x08, 0x8C, 0x09, 0x34, 0x02, 0x31, 0x08, 0x8D, + 0x09, 0xE4, 0x09, 0x00, 0x09, 0x00, 0x02, 0x31, 0x08, 0x8E, + 0x09, 0xCD, 0x02, 0x31, 0x08, 0x8F, 0x09, 0x34, 0x02, 0x31, + 0x08, 0x90, 0x09, 0x00, 0x02, 0x31, 0x08, 0x91, 0x09, 0xA2, + 0x09, 0x00, 0x09, 0x00, 0x02, 0x31, 0x08, 0x92, 0x09, 0xCD, + 0x02, 0x31, 0x08, 0x93, 0x09, 0x34, 0x02, 0x31, 0x08, 0x94, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x95, 0x09, 0xA1, 0x09, 0x00, + 0x09, 0x00, 0x02, 0x31, 0x08, 0x96, 0x09, 0xCD, 0x02, 0x31, + 0x08, 0x97, 0x09, 0x34, 0x02, 0x31, 0x08, 0x98, 0x09, 0x12, + 0x02, 0x31, 0x08, 0x99, 0x09, 0xE3, 0x01, 0x80, 0x17, 0x00, + 0x04, 0x00, 0x05, 0x10, 0x02, 0x01, 0x01, 0x00, 0x12, 0x44, + 0x01, 0x80, 0x60, 0x15, 0x01, 0x00, 0x10, 0x20, 0x11, 0x00, + 0x5E, 0x02, 0x01, 0x80, 0x0D, 0x01, 0x01, 0x00 +}; + +static const struct firmware st_lsm6dsox_mlc_preload = { + .size = sizeof(mlcdata), + .data = mlcdata +}; + +/* Converts MLC odr to main sensor trigger odr (acc) */ +static const uint16_t mlc_odr_data[] = { + [0x00] = 0, + [0x01] = 12, + [0x02] = 26, + [0x03] = 52, + [0x04] = 104, + [0x05] = 208, + [0x06] = 416, + [0x07] = 833, +}; + +static +struct iio_dev *st_lsm6dsox_mlc_alloc_iio_dev(struct st_lsm6dsox_hw *hw, + enum st_lsm6dsox_sensor_id id); + +static const unsigned long st_lsm6dsox_mlc_available_scan_masks[] = { + 0x1, 0x0 +}; + +static inline int +st_lsm6dsox_read_page_locked(struct st_lsm6dsox_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + st_lsm6dsox_set_page_access(hw, true, ST_LSM6DSOX_REG_FUNC_CFG_MASK); + err = regmap_bulk_read(hw->regmap, addr, val, len); + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsox_write_page_locked(struct st_lsm6dsox_hw *hw, unsigned int addr, + unsigned int *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsox_set_page_access(hw, true, ST_LSM6DSOX_REG_FUNC_CFG_MASK); + err = regmap_bulk_write(hw->regmap, addr, val, len); + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsox_update_page_bits_locked(struct st_lsm6dsox_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsox_set_page_access(hw, true, ST_LSM6DSOX_REG_FUNC_CFG_MASK); + err = regmap_update_bits(hw->regmap, addr, mask, val); + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static int st_lsm6dsox_mlc_enable_sensor(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + struct st_lsm6dsox_hw *hw = sensor->hw; + int i, id, err = 0; + + /* enable acc sensor as trigger */ + err = st_lsm6dsox_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + if (sensor->status == ST_LSM6DSOX_MLC_ENABLED) { + int value; + + value = enable ? hw->mlc_config->mlc_int_mask : 0; + err = st_lsm6dsox_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_lsm6dsox_update_page_bits_locked(hw, + ST_LSM6DSOX_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSOX_MLC_EN_MASK, + ST_LSM6DSOX_SHIFT_VAL(enable, + ST_LSM6DSOX_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_LSM6DSOX_FSM_ENABLED) { + int value[2]; + + value[0] = enable ? hw->mlc_config->fsm_int_mask[0] : 0; + value[1] = enable ? hw->mlc_config->fsm_int_mask[1] : 0; + err = st_lsm6dsox_write_page_locked(hw, + hw->mlc_config->fsm_int_addr[0], + &value[0], 2); + if (err < 0) + return err; + + /* enable fsm core */ + for (i = 0; i < ST_LSM6DSOX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsox_fsm_sensor_list[i]; + if (hw->enable_mask & BIT(id)) + break; + } + + /* check for any other fsm already enabled */ + if (enable || i == ST_LSM6DSOX_FSM_MAX_NUMBER) { + err = st_lsm6dsox_update_page_bits_locked(hw, + ST_LSM6DSOX_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSOX_FSM_EN_MASK, + ST_LSM6DSOX_SHIFT_VAL(enable, + ST_LSM6DSOX_FSM_EN_MASK)); + if (err < 0) + return err; + } + + dev_info(sensor->hw->dev, + "Enabling FSM sensor %d to %d (INT %x-%x)\n", + sensor->id, enable, value[0], value[1]); + } 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_lsm6dsox_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_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsox_mlc_enable_sensor(sensor, state); +} + +static int st_lsm6dsox_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_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsox_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/* + * st_lsm6dsox_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_lsm6dsox_verify_mlc_fsm_support(const struct firmware *fw, + struct st_lsm6dsox_hw *hw) +{ + bool stmc_page = false; + uint8_t reg, val; + int i = 0; + + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == 0x01 && val == 0x80) { + stmc_page = true; + } else if (reg == 0x01 && val == 0x00) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSOX_MLC_INT1_ADDR: + case ST_LSM6DSOX_MLC_INT2_ADDR: + if (!hw->settings->st_mlc_probe) + return -ENODEV; + break; + case ST_LSM6DSOX_FSM_INT1_A_ADDR: + case ST_LSM6DSOX_FSM_INT2_A_ADDR: + case ST_LSM6DSOX_FSM_INT1_B_ADDR: + case ST_LSM6DSOX_FSM_INT2_B_ADDR: + if (!hw->settings->st_fsm_probe) + return -ENODEV; + break; + default: + break; + } + } + } + + return 0; +} + +/* parse and program mlc fragments */ +static int st_lsm6dsox_program_mlc(const struct firmware *fw, + struct st_lsm6dsox_hw *hw) +{ + uint8_t mlc_int = 0, mlc_num = 0, fsm_num = 0, skip = 0; + uint8_t fsm_int[2] = { 0, 0 }; + uint8_t reg, val, req_odr = 0; + int int_pin, ret, i = 0; + bool stmc_page = false; + + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == 0x01 && val == 0x80) { + stmc_page = true; + } else if (reg == 0x01 && val == 0x00) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSOX_MLC_INT1_ADDR: + case ST_LSM6DSOX_MLC_INT2_ADDR: + mlc_int |= val; + mlc_num++; + skip = 1; + break; + case ST_LSM6DSOX_FSM_INT1_A_ADDR: + case ST_LSM6DSOX_FSM_INT2_A_ADDR: + fsm_int[0] |= val; + fsm_num++; + skip = 1; + break; + case ST_LSM6DSOX_FSM_INT1_B_ADDR: + case ST_LSM6DSOX_FSM_INT2_B_ADDR: + fsm_int[1] |= val; + fsm_num++; + skip = 1; + break; + case ST_LSM6DSOX_EMB_FUNC_EN_B_ADDR: + skip = 1; + break; + default: + break; + } + } else if (reg == 0x10) { + /* save requested odr and skip write to reg */ + req_odr = max_t(uint8_t, req_odr, ((val >> 4) & 0x07)); + 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_LSM6DSOX_MLC_MAX_NUMBER || + fsm_num >= ST_LSM6DSOX_FSM_MAX_NUMBER) + break; + } + + hw->mlc_config->bin_len = fw->size; + + ret = st_lsm6dsox_of_get_pin(hw, &int_pin); + if (ret < 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; + } + + if (mlc_num) { + hw->mlc_config->mlc_int_mask = mlc_int; + + hw->mlc_config->mlc_int_addr = (int_pin == 1 ? + ST_LSM6DSOX_MLC_INT1_ADDR : + ST_LSM6DSOX_MLC_INT2_ADDR); + + hw->mlc_config->status |= ST_LSM6DSOX_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[0] = fsm_int[0]; + hw->mlc_config->fsm_int_mask[1] = fsm_int[1]; + + hw->mlc_config->fsm_int_addr[0] = (int_pin == 1 ? + ST_LSM6DSOX_FSM_INT1_A_ADDR : + ST_LSM6DSOX_FSM_INT2_A_ADDR); + hw->mlc_config->fsm_int_addr[1] = (int_pin == 1 ? + ST_LSM6DSOX_FSM_INT1_B_ADDR : + ST_LSM6DSOX_FSM_INT2_B_ADDR); + + hw->mlc_config->status |= ST_LSM6DSOX_FSM_ENABLED; + hw->mlc_config->fsm_configured += fsm_num; + hw->mlc_config->requested_odr = mlc_odr_data[req_odr]; + } + + return fsm_num + mlc_num; +} + +static void st_lsm6dsox_mlc_update(const struct firmware *fw, + void *context) +{ + struct st_lsm6dsox_hw *hw = context; + enum st_lsm6dsox_sensor_id id; + int ret, i; + + if (!fw) { + dev_err(hw->dev, "could not get binary firmware\n"); + return; + } + + ret = st_lsm6dsox_verify_mlc_fsm_support(fw, hw); + if (ret) { + dev_err(hw->dev, "invalid file format for device\n"); + return; + } + + ret = st_lsm6dsox_program_mlc(fw, hw); + if (ret > 0) { + u16 fsm_mask = *(u16 *)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_LSM6DSOX_MLC_MAX_NUMBER; i++) { + if (mlc_mask & BIT(i)) { + id = st_lsm6dsox_mlc_sensor_list[i]; + hw->iio_devs[id] = + st_lsm6dsox_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_LSM6DSOX_FSM_MAX_NUMBER; i++) { + if (fsm_mask & BIT(i)) { + id = st_lsm6dsox_fsm_sensor_list[i]; + hw->iio_devs[id] = + st_lsm6dsox_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: + if (hw->preload_mlc) { + hw->preload_mlc = 0; + + return; + } + + release_firmware(fw); +} + +static int st_lsm6dsox_mlc_flush_all(struct st_lsm6dsox_hw *hw) +{ + struct st_lsm6dsox_sensor *sensor_mlc; + struct iio_dev *iio_dev; + int ret = 0, id; + + for (id = ST_LSM6DSOX_ID_MLC_0; id < ST_LSM6DSOX_ID_MAX; id++) { + iio_dev = hw->iio_devs[id]; + if (!iio_dev) + continue; + + sensor_mlc = iio_priv(iio_dev); + ret = st_lsm6dsox_mlc_enable_sensor(sensor_mlc, false); + if (ret < 0) + break; + + iio_device_unregister(iio_dev); + kfree(iio_dev->channels); + iio_device_free(iio_dev); + hw->iio_devs[id] = NULL; + } + + return ret; +} + +static ssize_t st_lsm6dsox_mlc_info(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsox_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_lsm6dsox_mlc_get_version(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "mlc loader Version %s\n", + ST_LSM6DSOX_MLC_LOADER_VERSION); +} + +static ssize_t st_lsm6dsox_mlc_flush(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsox_hw *hw = sensor->hw; + int ret; + + ret = st_lsm6dsox_mlc_flush_all(hw); + memset(hw->mlc_config, 0, sizeof(*hw->mlc_config)); + + return ret < 0 ? ret : size; +} + +static ssize_t st_lsm6dsox_mlc_upload_firmware(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err; + + err = request_firmware_nowait(THIS_MODULE, true, + LSM6DSOX_MLC_FIRMWARE_NAME, + dev, GFP_KERNEL, + sensor->hw, + st_lsm6dsox_mlc_update); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6dsox_mlc_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsox_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%d\n", hw->mlc_config->requested_odr); +} + +static IIO_DEVICE_ATTR(mlc_info, 0444, + st_lsm6dsox_mlc_info, NULL, 0); +static IIO_DEVICE_ATTR(mlc_flush, 0200, + NULL, st_lsm6dsox_mlc_flush, 0); +static IIO_DEVICE_ATTR(mlc_version, 0444, + st_lsm6dsox_mlc_get_version, NULL, 0); +static IIO_DEVICE_ATTR(load_mlc, 0200, + NULL, st_lsm6dsox_mlc_upload_firmware, 0); +static IIO_DEVICE_ATTR(mlc_odr, 0444, + st_lsm6dsox_mlc_odr, NULL, 0); + +static struct attribute *st_lsm6dsox_mlc_event_attributes[] = { + &iio_dev_attr_mlc_info.dev_attr.attr, + &iio_dev_attr_mlc_version.dev_attr.attr, + &iio_dev_attr_load_mlc.dev_attr.attr, + &iio_dev_attr_mlc_flush.dev_attr.attr, + &iio_dev_attr_mlc_odr.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsox_mlc_event_attribute_group = { + .attrs = st_lsm6dsox_mlc_event_attributes, +}; + +static const struct iio_info st_lsm6dsox_mlc_event_info = { + .attrs = &st_lsm6dsox_mlc_event_attribute_group, + .read_event_config = st_lsm6dsox_mlc_read_event_config, + .write_event_config = st_lsm6dsox_mlc_write_event_config, +}; + +static ssize_t st_lsm6dsox_mlc_x_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return scnprintf(buf, PAGE_SIZE, "%d.%02d\n", + sensor->odr, sensor->uodr); +} + +static IIO_DEVICE_ATTR(mlc_x_odr, 0444, + st_lsm6dsox_mlc_x_odr, NULL, 0); + +static struct attribute *st_lsm6dsox_mlc_x_event_attributes[] = { + &iio_dev_attr_mlc_x_odr.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsox_mlc_x_event_attribute_group = { + .attrs = st_lsm6dsox_mlc_x_event_attributes, +}; +static const struct iio_info st_lsm6dsox_mlc_x_event_info = { + .attrs = &st_lsm6dsox_mlc_x_event_attribute_group, + .read_event_config = st_lsm6dsox_mlc_read_event_config, + .write_event_config = st_lsm6dsox_mlc_write_event_config, +}; + +static +struct iio_dev *st_lsm6dsox_mlc_alloc_iio_dev(struct st_lsm6dsox_hw *hw, + enum st_lsm6dsox_sensor_id id) +{ + struct st_lsm6dsox_sensor *sensor; + struct iio_chan_spec *channels; + struct iio_dev *iio_dev; + + /* devm management only for ST_LSM6DSOX_ID_MLC */ + if (id == ST_LSM6DSOX_ID_MLC) { + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + } else { +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,9,0) + 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; + sensor->pm = ST_LSM6DSOX_NO_MODE; + + switch (id) { + case ST_LSM6DSOX_ID_MLC: { + const struct iio_chan_spec st_lsm6dsox_mlc_channels[] = { + ST_LSM6DSOX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = devm_kzalloc(hw->dev, + sizeof(st_lsm6dsox_mlc_channels), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsox_mlc_channels, + sizeof(st_lsm6dsox_mlc_channels)); + + iio_dev->available_scan_masks = + st_lsm6dsox_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_mlc_channels); + iio_dev->info = &st_lsm6dsox_mlc_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc", hw->dev_name); + break; + } + case ST_LSM6DSOX_ID_MLC_0: + case ST_LSM6DSOX_ID_MLC_1: + case ST_LSM6DSOX_ID_MLC_2: + case ST_LSM6DSOX_ID_MLC_3: + case ST_LSM6DSOX_ID_MLC_4: + case ST_LSM6DSOX_ID_MLC_5: + case ST_LSM6DSOX_ID_MLC_6: + case ST_LSM6DSOX_ID_MLC_7: { + const struct iio_chan_spec st_lsm6dsox_mlc_x_ch[] = { + ST_LSM6DSOX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsox_mlc_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsox_mlc_x_ch, + sizeof(st_lsm6dsox_mlc_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsox_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_mlc_x_ch); + iio_dev->info = &st_lsm6dsox_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc_%d", hw->dev_name, + id - ST_LSM6DSOX_ID_MLC_0); + sensor->outreg_addr = ST_LSM6DSOX_REG_MLC0_SRC_ADDR + + id - ST_LSM6DSOX_ID_MLC_0; + sensor->status = ST_LSM6DSOX_MLC_ENABLED; + sensor->odr = hw->mlc_config->requested_odr; + sensor->uodr = 0; + + break; + } + case ST_LSM6DSOX_ID_FSM_0: + case ST_LSM6DSOX_ID_FSM_1: + case ST_LSM6DSOX_ID_FSM_2: + case ST_LSM6DSOX_ID_FSM_3: + case ST_LSM6DSOX_ID_FSM_4: + case ST_LSM6DSOX_ID_FSM_5: + case ST_LSM6DSOX_ID_FSM_6: + case ST_LSM6DSOX_ID_FSM_7: + case ST_LSM6DSOX_ID_FSM_8: + case ST_LSM6DSOX_ID_FSM_9: + case ST_LSM6DSOX_ID_FSM_10: + case ST_LSM6DSOX_ID_FSM_11: + case ST_LSM6DSOX_ID_FSM_12: + case ST_LSM6DSOX_ID_FSM_13: + case ST_LSM6DSOX_ID_FSM_14: + case ST_LSM6DSOX_ID_FSM_15: { + const struct iio_chan_spec st_lsm6dsox_fsm_x_ch[] = { + ST_LSM6DSOX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsox_fsm_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsox_fsm_x_ch, + sizeof(st_lsm6dsox_fsm_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsox_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsox_fsm_x_ch); + iio_dev->info = &st_lsm6dsox_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_fsm_%d", hw->dev_name, + id - ST_LSM6DSOX_ID_FSM_0); + sensor->outreg_addr = ST_LSM6DSOX_FSM_OUTS1_ADDR + + id - ST_LSM6DSOX_ID_FSM_0; + sensor->status = ST_LSM6DSOX_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_lsm6dsox_mlc_check_status(struct st_lsm6dsox_hw *hw) +{ + struct st_lsm6dsox_sensor *sensor; + u8 i, mlc_status, id, event[16]; + struct iio_dev *iio_dev; + __le16 __fsm_status = 0; + u16 fsm_status; + int err = 0; + + if (hw->mlc_config->status & ST_LSM6DSOX_MLC_ENABLED) { + err = st_lsm6dsox_read_locked(hw, + ST_LSM6DSOX_MLC_STATUS_MAINPAGE, + (void *)&mlc_status, 1); + if (err) + return err; + + if (mlc_status) { + for (i = 0; i < ST_LSM6DSOX_MLC_MAX_NUMBER; i++) { + id = st_lsm6dsox_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_lsm6dsox_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_LSM6DSOX_FSM_ENABLED) { + err = st_lsm6dsox_read_locked(hw, + ST_LSM6DSOX_FSM_STATUS_A_MAINPAGE, + (void *)&__fsm_status, 2); + if (err) + return err; + + fsm_status = le16_to_cpu(__fsm_status); + if (fsm_status) { + for (i = 0; i < ST_LSM6DSOX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsox_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_lsm6dsox_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_lsm6dsox_mlc_probe(struct st_lsm6dsox_hw *hw) +{ + hw->iio_devs[ST_LSM6DSOX_ID_MLC] = + st_lsm6dsox_mlc_alloc_iio_dev(hw, ST_LSM6DSOX_ID_MLC); + if (!hw->iio_devs[ST_LSM6DSOX_ID_MLC]) + return -ENOMEM; + + hw->mlc_config = devm_kzalloc(hw->dev, + sizeof(struct st_lsm6dsox_mlc_config_t), + GFP_KERNEL); + if (!hw->mlc_config) + return -ENOMEM; + + return 0; +} + +int st_lsm6dsox_mlc_remove(struct device *dev) +{ + struct st_lsm6dsox_hw *hw = dev_get_drvdata(dev); + + return st_lsm6dsox_mlc_flush_all(hw); +} +EXPORT_SYMBOL(st_lsm6dsox_mlc_remove); + +int st_lsm6dsox_mlc_init_preload(struct st_lsm6dsox_hw *hw) +{ + hw->preload_mlc = 1; + st_lsm6dsox_mlc_update(&st_lsm6dsox_mlc_preload, hw); + + return 0; +} diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_shub.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_shub.c new file mode 100644 index 000000000000..987ac83cb436 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_shub.c @@ -0,0 +1,1102 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox sensor hub library driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsox.h" + +#define ST_LSM6DSOX_MAX_SLV_NUM 2 + +/** + * @struct st_lsm6dsox_ext_pwr + * @brief External device Power Management description + * reg: Generic sensor register description. + * off_val: Value to write into register to power off external sensor. + * on_val: Value to write into register for power on external sensor. + */ +struct st_lsm6dsox_ext_pwr { + struct st_lsm6dsox_reg reg; + u8 off_val; + u8 on_val; +}; + +/** + * @struct st_lsm6dsox_ext_dev_settings + * @brief External sensor descritor entry + * i2c_addr: External I2C device address (max two). + * wai_addr: Device ID address. + * wai_val: Device ID value. + * odr_table: ODR sensor table. + * fs_table: Full scale table. + * temp_comp_reg: Temperature compensation registers. + * pwr_table: External device Power Management description. + * off_canc_reg: Offset cancellation registers. + * bdu_reg: Block Data Update registers. + * ext_available_scan_masks: IIO device scan mask. + * ext_channels:IIO device channel specifications. + * ext_chan_depth: Max number of IIO device channel specifications. + * data_len: Sensor output data len. + */ +struct st_lsm6dsox_ext_dev_settings { + u8 i2c_addr[2]; + u8 wai_addr; + u8 wai_val; + struct st_lsm6dsox_odr_table_entry odr_table; + struct st_lsm6dsox_fs_table_entry fs_table; + struct st_lsm6dsox_reg temp_comp_reg; + struct st_lsm6dsox_ext_pwr pwr_table; + struct st_lsm6dsox_reg off_canc_reg; + struct st_lsm6dsox_reg bdu_reg; + unsigned long ext_available_scan_masks[2]; + const struct iio_chan_spec ext_channels[5]; + u8 ext_chan_depth; + u8 data_len; +}; + +static const struct st_lsm6dsox_ext_dev_settings st_lsm6dsox_ext_dev_table[] = { + { + /* LIS2MDL */ + .i2c_addr = { 0x1e }, + .wai_addr = 0x4f, + .wai_val = 0x40, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + /* + * added 5Hz for CTS coverage, reg value is the same + * for 5 and 10 Hz + */ + .odr_avl[0] = { 5, 1, 0x0, 0 }, + .odr_avl[1] = { 10, 0, 0x0, 0 }, + .odr_avl[2] = { 20, 0, 0x1, 0 }, + .odr_avl[3] = { 50, 0, 0x2, 0 }, + .odr_avl[4] = { 100, 0, 0x3, 0 }, + }, + .fs_table = { + .fs_len = 1, + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + }, + .temp_comp_reg = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc_reg = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu_reg = { + .addr = 0x62, + .mask = BIT(4), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_LSM6DSOX_DATA_CHANNEL(IIO_MAGN, 0x68, + 1, IIO_MOD_X, 0, + 16, 16, 's', NULL), + .ext_channels[1] = ST_LSM6DSOX_DATA_CHANNEL(IIO_MAGN, 0x6a, + 1, IIO_MOD_Y, 1, + 16, 16, 's', NULL), + .ext_channels[2] = ST_LSM6DSOX_DATA_CHANNEL(IIO_MAGN, 0x6c, + 1, IIO_MOD_Z, 2, + 16, 16, 's', NULL), + .ext_channels[3] = ST_LSM6DSOX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + { + /* LIS3MDL */ + .i2c_addr = { 0x1c, 0x1e }, + .wai_addr = 0x0f, + .wai_val = 0x3d, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x20, + .mask = GENMASK(4, 2), + }, + .odr_avl[0] = { 5, 0, 0x3, 0 }, + .odr_avl[1] = { 10, 0, 0x3, 0 }, + .odr_avl[2] = { 20, 0, 0x4, 0 }, + .odr_avl[3] = { 40, 0, 0x5, 0 }, + .odr_avl[4] = { 80, 0, 0x6, 0 }, + .odr_avl[5] = { 100, 0, 0x7, 0 }, + }, + .fs_table = { + .fs_len = 4, + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .fs_avl[0] = { + .gain = 6842, + .val = 0x0, + }, + .fs_avl[1] = { + .gain = 3421, + .val = 0x1, + }, + .fs_avl[2] = { + .gain = 2281, + .val = 0x2, + }, + .fs_avl[3] = { + .gain = 1711, + .val = 0x3, + }, + }, + .temp_comp_reg = { + .addr = 0x20, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x22, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .bdu_reg = { + .addr = 0x24, + .mask = BIT(6), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_LSM6DSOX_DATA_CHANNEL(IIO_MAGN, 0x28, + 1, IIO_MOD_X, 0, + 16, 16, 's', NULL), + .ext_channels[1] = ST_LSM6DSOX_DATA_CHANNEL(IIO_MAGN, 0x2a, + 1, IIO_MOD_Y, 1, + 16, 16, 's', NULL), + .ext_channels[2] = ST_LSM6DSOX_DATA_CHANNEL(IIO_MAGN, 0x2c, + 1, IIO_MOD_Z, 2, + 16, 16, 's', NULL), + .ext_channels[3] = ST_LSM6DSOX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + { + /* LPS22HB */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb1, + .odr_table = { + .size = 4, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1, 0 }, + .odr_avl[1] = { 10, 0, 0x2, 0 }, + .odr_avl[2] = { 25, 0, 0x3, 0 }, + .odr_avl[3] = { 50, 0, 0x4, 0 }, + }, + .fs_table = { + .fs_len = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_LSM6DSOX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u', NULL), + .ext_channels[1] = ST_LSM6DSOX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, + { + /* LPS22HH */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb3, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1, 0 }, + .odr_avl[1] = { 10, 0, 0x2, 0 }, + .odr_avl[2] = { 25, 0, 0x3, 0 }, + .odr_avl[3] = { 50, 0, 0x4, 0 }, + .odr_avl[4] = { 100, 0, 0x6, 0 }, + }, + .fs_table = { + .fs_len = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_LSM6DSOX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u', NULL), + .ext_channels[1] = ST_LSM6DSOX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, +}; + +/** + * Wait write trigger [SHUB] + * + * In write on external device register, each operation is triggered + * by accel/gyro data ready, this means that wait time depends on ODR + * plus i2c time + * NOTE: Be sure to enable Acc or Gyro before this operation + * + * @param hw: ST IMU MEMS hw instance. + */ +static inline void st_lsm6dsox_shub_wait_complete(struct st_lsm6dsox_hw *hw) +{ + struct st_lsm6dsox_sensor *sensor; + int odr, uodr; + + sensor = iio_priv(hw->iio_devs[ST_LSM6DSOX_ID_ACC]); + + /* check if acc is enabled (it should be) */ + if (hw->enable_mask & BIT(ST_LSM6DSOX_ID_ACC)) { + odr = sensor->odr; + uodr = sensor->uodr; + } else { + odr = 12; + uodr = 500000; + } + + msleep((2000000000U / (odr * 1000000 + uodr)) + 1); +} + +/** + * Read from sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_read_reg(struct st_lsm6dsox_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = regmap_bulk_read(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dsox_set_page_access(hw, false, + ST_LSM6DSOX_REG_SHUB_REG_MASK); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Write to sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_write_reg(struct st_lsm6dsox_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = regmap_bulk_write(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dsox_set_page_access(hw, false, + ST_LSM6DSOX_REG_SHUB_REG_MASK); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable sensor hub interface [SHUB] + * + * NOTE: uses page_lock + * + * @param sensor: ST IMU sensor instance + * @param enable: Master Enable/Disable. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_master_enable(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + struct st_lsm6dsox_hw *hw = sensor->hw; + int err; + + /* enable acc sensor as trigger */ + err = st_lsm6dsox_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsox_set_page_access(hw, true, + ST_LSM6DSOX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = __st_lsm6dsox_write_with_mask(hw, + ST_LSM6DSOX_REG_MASTER_CONFIG_ADDR, + ST_LSM6DSOX_REG_MASTER_ON_MASK, + enable); + + st_lsm6dsox_set_page_access(hw, false, ST_LSM6DSOX_REG_SHUB_REG_MASK); + +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Read sensor data register from shub interface + * + * NOTE: use SLV3 i2c slave for one-shot read operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_read(struct st_lsm6dsox_sensor *sensor, u8 addr, + u8 *data, int len) +{ + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsox_hw *hw = sensor->hw; + u8 out_addr = ST_LSM6DSOX_REG_SLV0_OUT_ADDR + hw->ext_data_len; + u8 config[3]; + int err; + + config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[1] = addr; + config[2] = len & 0x7; + + err = st_lsm6dsox_shub_write_reg(hw, ST_LSM6DSOX_REG_SLV3_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsox_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsox_shub_wait_complete(hw); + + err = st_lsm6dsox_shub_read_reg(hw, out_addr, data, len & 0x7); + + st_lsm6dsox_shub_master_enable(sensor, false); + + memset(config, 0, sizeof(config)); + + return st_lsm6dsox_shub_write_reg(hw, ST_LSM6DSOX_REG_SLV3_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface + * + * NOTE: use SLV0 i2c slave for write operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_write(struct st_lsm6dsox_sensor *sensor, u8 addr, + u8 *data, int len) +{ + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsox_hw *hw = sensor->hw; + u8 mconfig = ST_LSM6DSOX_REG_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + u8 config[3] = {}; + int err, i; + + /* AuxSens = 3 + wr once + pull up configuration */ + err = st_lsm6dsox_shub_write_reg(hw, + ST_LSM6DSOX_REG_MASTER_CONFIG_ADDR, + &mconfig, sizeof(mconfig)); + if (err < 0) + return err; + + config[0] = ext_info->ext_dev_i2c_addr << 1; + for (i = 0; i < len; i++) { + config[1] = addr + i; + + err = st_lsm6dsox_shub_write_reg(hw, + ST_LSM6DSOX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsox_shub_write_reg(hw, + ST_LSM6DSOX_REG_DATAWRITE_SLV0_ADDR, + &data[i], 1); + if (err < 0) + return err; + + err = st_lsm6dsox_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsox_shub_wait_complete(hw); + + st_lsm6dsox_shub_master_enable(sensor, false); + } + + return st_lsm6dsox_shub_write_reg(hw, ST_LSM6DSOX_REG_SLV0_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface using register bitmask + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param mask: Register bitmask. + * @param val: Data buffer. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_write_with_mask(struct st_lsm6dsox_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_lsm6dsox_shub_read(sensor, addr, &data, sizeof(data)); + if (err < 0) + return err; + + data = (data & ~mask) | ST_LSM6DSOX_SHIFT_VAL(val, mask); + + return st_lsm6dsox_shub_write(sensor, addr, &data, sizeof(data)); +} + +/** + * Configure external sensor connected on master I2C interface + * + * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsox_shub_config_channels(struct st_lsm6dsox_sensor *sensor, + bool enable) +{ + struct st_lsm6dsox_ext_dev_info *ext_info; + struct st_lsm6dsox_hw *hw = sensor->hw; + struct st_lsm6dsox_sensor *cur_sensor; + u8 config[6] = {}, enable_mask; + int i, j = 0; + + enable_mask = enable ? hw->enable_mask | BIT(sensor->id) + : hw->enable_mask & ~BIT(sensor->id); + + for (i = ST_LSM6DSOX_ID_EXT0; i <= ST_LSM6DSOX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT(cur_sensor->id))) + continue; + + ext_info = &cur_sensor->ext_dev_info; + config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[j + 1] = + ext_info->ext_dev_settings->ext_channels[0].address; + config[j + 2] = ST_LSM6DSOX_REG_BATCH_EXT_SENS_EN_MASK | + (ext_info->ext_dev_settings->data_len & + ST_LSM6DSOX_REG_SLAVE_NUMOP_MASK); + j += 3; + } + + return st_lsm6dsox_shub_write_reg(hw, ST_LSM6DSOX_REG_SLV1_ADDR, + config, sizeof(config)); +} + +/** + * Get a valid ODR [SHUB] + * + * Check a valid ODR closest to the passed value + * + * @param sensor: SST IMU sensor instance. + * @param odr: ODR value (in Hz). + * @param val: ODR register value data pointer. + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dsox_shub_get_odr_val(struct st_lsm6dsox_sensor *sensor, + u16 odr, u8 *val) +{ + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) + if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr) + break; + + if (i == ext_info->ext_dev_settings->odr_table.size) + return -EINVAL; + + *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val; + + /* set decimator for low ODR */ + sensor->decimator = + ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz; + sensor->dec_counter = 0; + + return 0; +} + +/** + * Set new ODR to sensor [SHUB] + * + * Set a valid ODR closest to the passed value + * + * @param sensor: ST IMU sensor instance + * @param odr: ODR value (in Hz). + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dsox_shub_set_odr(struct st_lsm6dsox_sensor *sensor, u16 odr) +{ + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsox_hw *hw = sensor->hw; + u8 odr_val; + int err; + + err = st_lsm6dsox_shub_get_odr_val(sensor, odr, &odr_val); + if (err < 0) + return err; + + if (sensor->odr == odr && (hw->enable_mask & BIT(sensor->id))) + return 0; + + return st_lsm6dsox_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + odr_val); +} + +/** + * Enable or Disable sensor [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable or disable the sensor [true,false]. + * @return 0 if OK, negative value for ERROR + */ +int st_lsm6dsox_shub_set_enable(struct st_lsm6dsox_sensor *sensor, bool enable) +{ + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err; + + err = st_lsm6dsox_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + if (enable) { + err = st_lsm6dsox_shub_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + } else { + err = st_lsm6dsox_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + 0); + if (err < 0) + return err; + } + + if (ext_info->ext_dev_settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? ext_info->ext_dev_settings->pwr_table.on_val + : ext_info->ext_dev_settings->pwr_table.off_val; + err = st_lsm6dsox_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->pwr_table.reg.addr, + ext_info->ext_dev_settings->pwr_table.reg.mask, + val); + if (err < 0) + return err; + } + + return st_lsm6dsox_shub_master_enable(sensor, enable); +} + +static inline u32 st_lsm6dsox_get_unaligned_le24(const u8 *p) +{ + return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; +} + +/** + * Single sensor read operation [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param ch: IIO Channel. + * @param val: Output data register value. + * @return IIO_VAL_INT if OK, negative value for ERROR + */ +static int st_lsm6dsox_shub_read_oneshot(struct st_lsm6dsox_sensor *sensor, + struct iio_chan_spec const *ch, + int *val) +{ + int err, delay, len = ch->scan_type.realbits >> 3; + u8 data[4]; + + if (len > ARRAY_SIZE(data)) + return -ENOMEM; + + err = st_lsm6dsox_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsox_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + st_lsm6dsox_shub_set_enable(sensor, false); + + switch (len) { + case 3: + *val = (s32)st_lsm6dsox_get_unaligned_le24(data); + break; + case 2: + *val = (s16)get_unaligned_le16(data); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +/** + * Read Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param ch: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_lsm6dsox_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsox_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_lsm6dsox_shub_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * Write Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_lsm6dsox_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_lsm6dsox_shub_get_odr_val(sensor, val, &data); + if (!err) + sensor->odr = val; + break; + } + case IIO_CHAN_INFO_SCALE: + err = 0; + break; + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * Get a list of available sensor ODR [SHUB] + * + * List of available ODR returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_lsm6dsox_sysfs_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) { + u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +/** + * Get a list of available sensor Full Scale [SHUB] + * + * List of available Full Scale returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t st_lsm6dsox_sysfs_shub_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsox_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->fs_table.fs_len; i++) { + u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsox_sysfs_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_ext_scale_available, 0444, + st_lsm6dsox_sysfs_shub_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsox_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsox_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsox_get_watermark, + st_lsm6dsox_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsox_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsox_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_ext_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_lsm6dsox_ext_attribute_group = { + .attrs = st_lsm6dsox_ext_attributes, +}; + +static const struct iio_info st_lsm6dsox_ext_info = { + .attrs = &st_lsm6dsox_ext_attribute_group, + .read_raw = st_lsm6dsox_shub_read_raw, + .write_raw = st_lsm6dsox_shub_write_raw, +}; + +/** + * Allocate IIO device [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @param ext_settings: xternal sensor descritor entry. + * @param id: Sensor Identifier. + * @param i2c_addr: external I2C address on master bus. + * @return struct iio_dev *, NULL if ERROR + */ +static struct iio_dev *st_lsm6dsox_shub_alloc_iio_dev(struct st_lsm6dsox_hw *hw, + const struct st_lsm6dsox_ext_dev_settings *ext_settings, + enum st_lsm6dsox_sensor_id id, u8 i2c_addr) +{ + struct st_lsm6dsox_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; + iio_dev->available_scan_masks = ext_settings->ext_available_scan_masks; + iio_dev->info = &st_lsm6dsox_ext_info; + iio_dev->channels = ext_settings->ext_channels; + iio_dev->num_channels = ext_settings->ext_chan_depth; + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = ext_settings->odr_table.odr_avl[0].hz; + sensor->gain = ext_settings->fs_table.fs_avl[0].gain; + sensor->max_watermark = ST_LSM6DSOX_MAX_FIFO_DEPTH; + sensor->watermark = 1; + sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr; + sensor->ext_dev_info.ext_dev_settings = ext_settings; + sensor->decimator = 0; + sensor->dec_counter = 0; + sensor->pm = ST_LSM6DSOX_NO_MODE; + + switch (iio_dev->channels[0].type) { + case IIO_MAGN: + scnprintf(sensor->name, sizeof(sensor->name), "%s_magn", + hw->dev_name); + break; + case IIO_PRESSURE: + scnprintf(sensor->name, sizeof(sensor->name), "%s_press", + hw->dev_name); + break; + default: + scnprintf(sensor->name, sizeof(sensor->name), "%s_ext", + hw->dev_name); + break; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static int st_lsm6dsox_shub_init_remote_sensor(struct st_lsm6dsox_sensor *sensor) +{ + struct st_lsm6dsox_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err = 0; + + if (ext_info->ext_dev_settings->bdu_reg.addr) + err = st_lsm6dsox_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->bdu_reg.addr, + ext_info->ext_dev_settings->bdu_reg.mask, 1); + + if (ext_info->ext_dev_settings->temp_comp_reg.addr) + err = st_lsm6dsox_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->temp_comp_reg.addr, + ext_info->ext_dev_settings->temp_comp_reg.mask, 1); + + if (ext_info->ext_dev_settings->off_canc_reg.addr) + err = st_lsm6dsox_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->off_canc_reg.addr, + ext_info->ext_dev_settings->off_canc_reg.mask, 1); + + return err; +} + +/** + * Probe device function [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @return 0 if OK, negative for ERROR + */ +int st_lsm6dsox_shub_probe(struct st_lsm6dsox_hw *hw) +{ + const struct st_lsm6dsox_ext_dev_settings *settings; + struct st_lsm6dsox_sensor *acc_sensor, *sensor; + u8 config[3], data, num_ext_dev = 0; + enum st_lsm6dsox_sensor_id id; + int err, i = 0, j; + struct device_node *np = hw->dev->of_node; + + if (np && of_property_read_bool(np, "drive-pullup-shub")) { + dev_err(hw->dev, "enabling pull up on i2c master\n"); + err = st_lsm6dsox_shub_read_reg(hw, + ST_LSM6DSOX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + if (err < 0) + return err; + + data |= ST_LSM6DSOX_REG_SHUB_PU_EN_MASK; + err = st_lsm6dsox_shub_write_reg(hw, + ST_LSM6DSOX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + + if (err < 0) + return err; + + hw->i2c_master_pu = ST_LSM6DSOX_REG_SHUB_PU_EN_MASK; + } + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSOX_ID_ACC]); + while (i < ARRAY_SIZE(st_lsm6dsox_ext_dev_table) && + num_ext_dev < ST_LSM6DSOX_MAX_SLV_NUM) { + settings = &st_lsm6dsox_ext_dev_table[i]; + + for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) { + if (!settings->i2c_addr[j]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[j] << 1) | 1; + config[1] = settings->wai_addr; + config[2] = 1; + + err = st_lsm6dsox_shub_write_reg(hw, + ST_LSM6DSOX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsox_shub_master_enable(acc_sensor, + true); + if (err < 0) + return err; + + st_lsm6dsox_shub_wait_complete(hw); + + err = st_lsm6dsox_shub_read_reg(hw, + ST_LSM6DSOX_REG_SLV0_OUT_ADDR, + &data, sizeof(data)); + + st_lsm6dsox_shub_master_enable(acc_sensor, false); + + if (err < 0) + return err; + + if (data != settings->wai_val) + continue; + + id = ST_LSM6DSOX_ID_EXT0 + num_ext_dev; + hw->iio_devs[id] = st_lsm6dsox_shub_alloc_iio_dev(hw, + settings, id, + settings->i2c_addr[j]); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_lsm6dsox_shub_init_remote_sensor(sensor); + if (err < 0) + return err; + + num_ext_dev++; + hw->ext_data_len += settings->data_len; + break; + } + + i++; + } + + if (!num_ext_dev) + return 0; + + memset(config, 0, sizeof(config)); + err = st_lsm6dsox_shub_write_reg(hw, ST_LSM6DSOX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + /* AuxSens = 3 + wr once */ + data = ST_LSM6DSOX_REG_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + return st_lsm6dsox_shub_write_reg(hw, + ST_LSM6DSOX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_spi.c b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_spi.c new file mode 100644 index 000000000000..99d29741a02b --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsox/st_lsm6dsox_spi.c @@ -0,0 +1,94 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsox spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsox.h" + +static const struct regmap_config st_lsm6dsox_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsox_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_lsm6dsox_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_lsm6dsox_probe(&spi->dev, spi->irq, hw_id, regmap); +} + +static int st_lsm6dsox_spi_remove(struct spi_device *spi) +{ + int err = 0; + struct st_lsm6dsox_hw *hw = dev_get_drvdata(&spi->dev); + + if (hw->settings->st_mlc_probe) + err = st_lsm6dsox_mlc_remove(&spi->dev); + + return err; +} + +static const struct of_device_id st_lsm6dsox_spi_of_match[] = { + { + .compatible = "st,lsm6dso", + .data = (void *)ST_LSM6DSO_ID, + }, + { + .compatible = "st,lsm6dsox", + .data = (void *)ST_LSM6DSOX_ID, + }, + { + .compatible = "st,lsm6dso32", + .data = (void *)ST_LSM6DSO32_ID, + }, + { + .compatible = "st,lsm6dso32x", + .data = (void *)ST_LSM6DSO32X_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsox_spi_of_match); + +static const struct spi_device_id st_lsm6dsox_spi_id_table[] = { + { ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID }, + { ST_LSM6DSOX_DEV_NAME, ST_LSM6DSOX_ID }, + { ST_LSM6DSO32_DEV_NAME, ST_LSM6DSO32_ID }, + { ST_LSM6DSO32X_DEV_NAME, ST_LSM6DSO32X_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsox_spi_id_table); + +static struct spi_driver st_lsm6dsox_driver = { + .driver = { + .name = "st_" ST_LSM6DSOX_DEV_NAME "_spi", + .pm = &st_lsm6dsox_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsox_spi_of_match), + }, + .probe = st_lsm6dsox_spi_probe, + .remove = st_lsm6dsox_spi_remove, + .id_table = st_lsm6dsox_spi_id_table, +}; +module_spi_driver(st_lsm6dsox_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsox spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/Kconfig b/drivers/iio/stm/imu/st_lsm6dsrx/Kconfig new file mode 100644 index 000000000000..c5b3bea9e531 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/Kconfig @@ -0,0 +1,48 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM6DSRX + tristate "STMicroelectronics LSM6DSRX sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_TRIGGERED_BUFFER + select IIO_ST_LSM6DSRX_I2C if (I2C) + select IIO_ST_LSM6DSRX_SPI if (SPI_MASTER) + select IIO_ST_LSM6DSRX_I3C if (I3C) + help + Say yes here to build support for STMicroelectronics LSM6DSRX imu + sensor. + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsrx. + +config IIO_ST_LSM6DSRX_I2C + tristate + select REGMAP_I2C + depends on IIO_ST_LSM6DSRX + +config IIO_ST_LSM6DSRX_SPI + tristate + select REGMAP_SPI + depends on IIO_ST_LSM6DSRX + +config IIO_ST_LSM6DSRX_I3C + tristate + depends on IIO_ST_LSM6DSRX + select REGMAP_I3C + +config IIO_ST_LSM6DSRX_MLC_PRELOAD + bool "Preload some examples on MLC/FSM core" + depends on IIO_ST_LSM6DSRX + help + Select yes if you want to preload some examples on machine learning core + and finite state machine. + + The examples code is a 6D position recognition and is hardcoded in the + driver in the mlcdata structure. + +config IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP + bool "Enable async hw timestamp read" + depends on IIO_ST_LSM6DSRX + help + Enable async task that sends over hw timestamp events. diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/Makefile b/drivers/iio/stm/imu/st_lsm6dsrx/Makefile new file mode 100644 index 000000000000..bbafc8505e5a --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/Makefile @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_lsm6dsrx-y := st_lsm6dsrx_core.o \ + st_lsm6dsrx_buffer.o \ + st_lsm6dsrx_mlc.o \ + st_lsm6dsrx_shub.o \ + st_lsm6dsrx_events.o \ + st_lsm6dsrx_embfunc.o + +st_lsm6dsrx-$(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) += st_lsm6dsrx_hwtimestamp.o + +obj-$(CONFIG_IIO_ST_LSM6DSRX) += st_lsm6dsrx.o +obj-$(CONFIG_IIO_ST_LSM6DSRX_I2C) += st_lsm6dsrx_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSRX_SPI) += st_lsm6dsrx_spi.o +obj-$(CONFIG_IIO_ST_LSM6DSRX_I3C) += st_lsm6dsrx_i3c.o +obj-$(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) += st_lsm6dsrx_hwtimestamp.o diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx.h b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx.h new file mode 100644 index 000000000000..bf0cd371a0c9 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx.h @@ -0,0 +1,1038 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dsrx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSRX_H +#define ST_LSM6DSRX_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#define ST_LSM6DSRX_ODR_EXPAND(odr, uodr) (((odr) * 1000000) + (uodr)) + +#define ST_LSM6DSR_DEV_NAME "lsm6dsr" +#define ST_LSM6DSRX_DEV_NAME "lsm6dsrx" + +#define ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DSRX_REG_SHUB_REG_MASK BIT(6) +#define ST_LSM6DSRX_REG_FUNC_CFG_MASK BIT(7) +#define ST_LSM6DSRX_REG_ACCESS_MASK GENMASK(7, 6) + +#define ST_LSM6DSRX_REG_FIFO_CTRL1_ADDR 0x07 +#define ST_LSM6DSRX_REG_FIFO_CTRL2_ADDR 0x08 +#define ST_LSM6DSRX_REG_FIFO_WTM_MASK GENMASK(8, 0) +#define ST_LSM6DSRX_REG_FIFO_WTM8_MASK BIT(0) +#define ST_LSM6DSRX_REG_FIFO_STATUS_DIFF GENMASK(9, 0) + +#define ST_LSM6DSRX_REG_FIFO_CTRL3_ADDR 0x09 +#define ST_LSM6DSRX_REG_BDR_XL_MASK GENMASK(3, 0) +#define ST_LSM6DSRX_REG_BDR_GY_MASK GENMASK(7, 4) + +#define ST_LSM6DSRX_REG_FIFO_CTRL4_ADDR 0x0a +#define ST_LSM6DSRX_REG_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_LSM6DSRX_REG_ODR_T_BATCH_MASK GENMASK(5, 4) +#define ST_LSM6DSRX_REG_DEC_TS_MASK GENMASK(7, 6) + +#define ST_LSM6DSRX_REG_INT1_CTRL_ADDR 0x0d +#define ST_LSM6DSRX_REG_INT2_CTRL_ADDR 0x0e +#define ST_LSM6DSRX_REG_FIFO_TH_MASK BIT(3) + +#define ST_LSM6DSRX_REG_WHOAMI_ADDR 0x0f +#define ST_LSM6DSRX_WHOAMI_VAL 0x6b + +#define ST_LSM6DSRX_CTRL1_XL_ADDR 0x10 +#define ST_LSM6DSRX_CTRL2_G_ADDR 0x11 + +#define ST_LSM6DSRX_REG_CTRL3_C_ADDR 0x12 +#define ST_LSM6DSRX_REG_SW_RESET_MASK BIT(0) +#define ST_LSM6DSRX_REG_PP_OD_MASK BIT(4) +#define ST_LSM6DSRX_REG_H_LACTIVE_MASK BIT(5) +#define ST_LSM6DSRX_REG_BDU_MASK BIT(6) +#define ST_LSM6DSRX_REG_BOOT_MASK BIT(7) + +#define ST_LSM6DSRX_REG_CTRL4_C_ADDR 0x13 +#define ST_LSM6DSRX_REG_DRDY_MASK BIT(3) + +#define ST_LSM6DSRX_REG_CTRL5_C_ADDR 0x14 +#define ST_LSM6DSRX_REG_ROUNDING_MASK GENMASK(6, 5) +#define ST_LSM6DSRX_REG_ST_G_MASK GENMASK(3, 2) +#define ST_LSM6DSRX_REG_ST_XL_MASK GENMASK(1, 0) + +#define ST_LSM6DSRX_SELFTEST_ACCEL_MIN 737 +#define ST_LSM6DSRX_SELFTEST_ACCEL_MAX 13934 +#define ST_LSM6DSRX_SELFTEST_GYRO_MIN 2142 +#define ST_LSM6DSRX_SELFTEST_GYRO_MAX 10000 + +#define ST_LSM6DSRX_SELF_TEST_DISABLED_VAL 0 +#define ST_LSM6DSRX_SELF_TEST_POS_SIGN_VAL 1 +#define ST_LSM6DSRX_SELF_TEST_NEG_ACCEL_SIGN_VAL 2 +#define ST_LSM6DSRX_SELF_TEST_NEG_GYRO_SIGN_VAL 3 + +#define ST_LSM6DSRX_REG_STATUS_MASTER_MAINPAGE_ADDR 0x39 +#define ST_LSM6DSRX_REG_STATUS_SENS_HUB_ENDOP_MASK BIT(0) + +#define ST_LSM6DSRX_REG_CTRL6_C_ADDR 0x15 +#define ST_LSM6DSRX_REG_XL_HM_MODE_MASK BIT(4) + +#define ST_LSM6DSRX_REG_CTRL7_G_ADDR 0x16 +#define ST_LSM6DSRX_REG_G_HM_MODE_MASK BIT(7) + +#define ST_LSM6DSRX_REG_CTRL10_C_ADDR 0x19 +#define ST_LSM6DSRX_REG_TIMESTAMP_EN_MASK BIT(5) + +#define ST_LSM6DSRX_REG_ALL_INT_SRC_ADDR 0x1a +#define ST_LSM6DSRX_FF_IA_MASK BIT(0) +#define ST_LSM6DSRX_WU_IA_MASK BIT(1) +#define ST_LSM6DSRX_SINGLE_TAP_MASK BIT(2) +#define ST_LSM6DSRX_DOUBLE_TAP_MASK BIT(3) +#define ST_LSM6DSRX_D6D_IA_MASK BIT(4) +#define ST_LSM6DSRX_SLEEP_CHANGE_MASK BIT(5) + +#define ST_LSM6DSRX_REG_WAKE_UP_SRC_ADDR 0x1b +#define ST_LSM6DSRX_WAKE_UP_EVENT_MASK GENMASK(3, 0) + +#define ST_LSM6DSRX_REG_D6D_SRC_ADDR 0x1d +#define ST_LSM6DSRX_D6D_EVENT_MASK GENMASK(5, 0) + +#define ST_LSM6DSRX_REG_STATUS_ADDR 0x1e +#define ST_LSM6DSRX_REG_STATUS_XLDA BIT(0) +#define ST_LSM6DSRX_REG_STATUS_GDA BIT(1) +#define ST_LSM6DSRX_REG_STATUS_TDA BIT(2) + +#define ST_LSM6DSRX_REG_OUT_TEMP_L_ADDR 0x20 + +#define ST_LSM6DSRX_REG_OUTX_L_A_ADDR 0x28 +#define ST_LSM6DSRX_REG_OUTY_L_A_ADDR 0x2a +#define ST_LSM6DSRX_REG_OUTZ_L_A_ADDR 0x2c + +#define ST_LSM6DSRX_REG_OUTX_L_G_ADDR 0x22 +#define ST_LSM6DSRX_REG_OUTY_L_G_ADDR 0x24 +#define ST_LSM6DSRX_REG_OUTZ_L_G_ADDR 0x26 + +#define ST_LSM6DSRX_REG_EMB_FUNC_STATUS_MAINPAGE_ADDR 0x35 +#define ST_LSM6DSRX_IS_STEP_DET_MASK BIT(3) +#define ST_LSM6DSRX_IS_TILT_MASK BIT(4) +#define ST_LSM6DSRX_IS_SIGMOT_MASK BIT(5) + +#define ST_LSM6DSRX_FSM_STATUS_A_MAINPAGE 0x36 +#define ST_LSM6DSRX_FSM_STATUS_B_MAINPAGE 0x37 +#define ST_LSM6DSRX_MLC_STATUS_MAINPAGE 0x38 + +#define ST_LSM6DSRX_REG_FIFO_STATUS1_ADDR 0x3a +#define ST_LSM6DSRX_REG_TIMESTAMP0_ADDR 0x40 +#define ST_LSM6DSRX_REG_TIMESTAMP2_ADDR 0x42 + +#define ST_LSM6DSRX_REG_TAP_CFG0_ADDR 0x56 +#define ST_LSM6DSRX_REG_LIR_MASK BIT(0) +#define ST_LSM6DSRX_REG_TAP_Z_EN_MASK BIT(1) +#define ST_LSM6DSRX_REG_TAP_Y_EN_MASK BIT(2) +#define ST_LSM6DSRX_REG_TAP_X_EN_MASK BIT(3) +#define ST_LSM6DSRX_REG_TAP_EN_MASK GENMASK(3, 1) + +#define ST_LSM6DSRX_REG_TAP_CFG1_ADDR 0x57 +#define ST_LSM6DSRX_TAP_THS_X_MASK GENMASK(4, 0) +#define ST_LSM6DSRX_TAP_PRIORITY_MASK GENMASK(7, 5) + +#define ST_LSM6DSRX_REG_TAP_CFG2_ADDR 0x58 +#define ST_LSM6DSRX_TAP_THS_Y_MASK GENMASK(4, 0) +#define ST_LSM6DSRX_INTERRUPTS_ENABLE_MASK BIT(7) + +#define ST_LSM6DSRX_REG_TAP_THS_6D_ADDR 0x59 +#define ST_LSM6DSRX_TAP_THS_Z_MASK GENMASK(4, 0) +#define ST_LSM6DSRX_SIXD_THS_MASK GENMASK(6, 5) + +#define ST_LSM6DSRX_REG_INT_DUR2_ADDR 0x5a +#define ST_LSM6DSRX_SHOCK_MASK GENMASK(1, 0) +#define ST_LSM6DSRX_QUIET_MASK GENMASK(3, 2) +#define ST_LSM6DSRX_DUR_MASK GENMASK(7, 4) + +#define ST_LSM6DSRX_REG_WAKE_UP_THS_ADDR 0x5b +#define ST_LSM6DSRX_WAKE_UP_THS_MASK GENMASK(5, 0) +#define ST_LSM6DSRX_SINGLE_DOUBLE_TAP_MASK BIT(7) + +#define ST_LSM6DSRX_REG_WAKE_UP_DUR_ADDR 0x5c +#define ST_LSM6DSRX_WAKE_UP_DUR_MASK GENMASK(6, 5) + +#define ST_LSM6DSRX_REG_FREE_FALL_ADDR 0x5d +#define ST_LSM6DSRX_FF_THS_MASK GENMASK(2, 0) + +#define ST_LSM6DSRX_REG_MD1_CFG_ADDR 0x5e +#define ST_LSM6DSRX_REG_MD2_CFG_ADDR 0x5f +#define ST_LSM6DSRX_REG_INT2_TIMESTAMP_MASK BIT(0) +#define ST_LSM6DSRX_REG_INT_EMB_FUNC_MASK BIT(1) +#define ST_LSM6DSRX_INT_6D_MASK BIT(2) +#define ST_LSM6DSRX_INT_DOUBLE_TAP_MASK BIT(3) +#define ST_LSM6DSRX_INT_FF_MASK BIT(4) +#define ST_LSM6DSRX_INT_WU_MASK BIT(5) +#define ST_LSM6DSRX_INT_SINGLE_TAP_MASK BIT(6) +#define ST_LSM6DSRX_INT_SLEEP_CHANGE_MASK BIT(7) + +#define ST_LSM6DSRX_INTERNAL_FREQ_FINE 0x63 + +#define ST_LSM6DSRX_REG_FIFO_DATA_OUT_TAG_ADDR 0x78 + +/* shub registers */ +#define ST_LSM6DSRX_REG_MASTER_CONFIG_ADDR 0x14 +#define ST_LSM6DSRX_REG_WRITE_ONCE_MASK BIT(6) +#define ST_LSM6DSRX_REG_SHUB_PU_EN_MASK BIT(3) +#define ST_LSM6DSRX_REG_MASTER_ON_MASK BIT(2) + +#define ST_LSM6DSRX_REG_SLV0_ADDR 0x15 +#define ST_LSM6DSRX_REG_SLV0_CFG 0x17 +#define ST_LSM6DSRX_REG_SLV1_ADDR 0x18 +#define ST_LSM6DSRX_REG_SLV2_ADDR 0x1b +#define ST_LSM6DSRX_REG_SLV3_ADDR 0x1e +#define ST_LSM6DSRX_REG_DATAWRITE_SLV0_ADDR 0x21 +#define ST_LSM6DSRX_REG_BATCH_EXT_SENS_EN_MASK BIT(3) +#define ST_LSM6DSRX_REG_SLAVE_NUMOP_MASK GENMASK(2, 0) + +#define ST_LSM6DSRX_REG_STATUS_MASTER_ADDR 0x22 +#define ST_LSM6DSRX_REG_SENS_HUB_ENDOP_MASK BIT(0) + +#define ST_LSM6DSRX_REG_SLV0_OUT_ADDR 0x02 + +/* embedded function registers */ +#define ST_LSM6DSRX_REG_EMB_FUNC_EN_A_ADDR 0x04 +#define ST_LSM6DSRX_REG_PEDO_EN_MASK BIT(3) +#define ST_LSM6DSRX_REG_TILT_EN_MASK BIT(4) +#define ST_LSM6DSRX_REG_SIGN_MOTION_EN_MASK BIT(5) + +#define ST_LSM6DSRX_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_LSM6DSRX_FSM_EN_MASK BIT(0) +#define ST_LSM6DSRX_MLC_EN_MASK BIT(4) + +#define ST_LSM6DSRX_REG_EMB_FUNC_INT1_ADDR 0x0a +#define ST_LSM6DSRX_INT_STEP_DETECTOR_MASK BIT(3) +#define ST_LSM6DSRX_INT_TILT_MASK BIT(4) +#define ST_LSM6DSRX_INT_SIG_MOT_MASK BIT(5) + +#define ST_LSM6DSRX_FSM_INT1_A_ADDR 0x0b + +#define ST_LSM6DSRX_FSM_INT1_B_ADDR 0x0c +#define ST_LSM6DSRX_MLC_INT1_ADDR 0x0d +#define ST_LSM6DSRX_REG_EMB_FUNC_INT2_ADDR 0x0e +#define ST_LSM6DSRX_FSM_INT2_A_ADDR 0x0f +#define ST_LSM6DSRX_FSM_INT2_B_ADDR 0x10 +#define ST_LSM6DSRX_MLC_INT2_ADDR 0x11 + +#define ST_LSM6DSRX_REG_MLC_STATUS_ADDR 0x15 + +#define ST_LSM6DSRX_REG_PAGE_RW_ADDR 0x17 +#define ST_LSM6DSRX_EMB_FUNC_LIR_MASK BIT(7) + +#define ST_LSM6DSRX_REG_EMB_FUNC_FIFO_CFG_ADDR 0x44 +#define ST_LSM6DSRX_PEDO_FIFO_EN_MASK BIT(6) + +#define ST_LSM6DSRX_FSM_ENABLE_A_ADDR 0x46 +#define ST_LSM6DSRX_FSM_ENABLE_B_ADDR 0x47 + +#define ST_LSM6DSRX_FSM_OUTS1_ADDR 0x4c + +#define ST_LSM6DSRX_REG_STEP_COUNTER_L_ADDR 0x62 + +#define ST_LSM6DSRX_REG_EMB_FUNC_SRC_ADDR 0x64 +#define ST_LSM6DSRX_STEPCOUNTER_BIT_SET_MASK BIT(2) +#define ST_LSM6DSRX_STEP_OVERFLOW_MASK BIT(3) +#define ST_LSM6DSRX_STEP_COUNT_DELTA_IA_MASK BIT(4) +#define ST_LSM6DSRX_STEP_DETECTED_MASK BIT(5) +#define ST_LSM6DSRX_PEDO_RST_STEP_MASK BIT(7) + +#define ST_LSM6DSRX_REG_MLC0_SRC_ADDR 0x70 + +/* Timestamp Tick 25us/LSB */ +#define ST_LSM6DSRX_TS_DELTA_NS 25000ULL + +/* Temperature in uC */ +#define ST_LSM6DSRX_TEMP_GAIN 256 +#define ST_LSM6DSRX_TEMP_OFFSET 6400 + +/* FIFO simple size and depth */ +#define ST_LSM6DSRX_SAMPLE_SIZE 6 +#define ST_LSM6DSRX_TS_SAMPLE_SIZE 4 +#define ST_LSM6DSRX_TAG_SIZE 1 +#define ST_LSM6DSRX_FIFO_SAMPLE_SIZE (ST_LSM6DSRX_SAMPLE_SIZE + \ + ST_LSM6DSRX_TAG_SIZE) +#define ST_LSM6DSRX_MAX_FIFO_DEPTH 416 + +#define ST_LSM6DSRX_DEFAULT_KTIME (200000000) +#define ST_LSM6DSRX_FAST_KTIME (5000000) + +#define ST_LSM6DSRX_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, \ +} + +static const struct iio_event_spec st_lsm6dsrx_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_lsm6dsrx_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_LSM6DSRX_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lsm6dsrx_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_LSM6DSRX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +enum st_lsm6dsrx_pm_t { + ST_LSM6DSRX_HP_MODE = 0, + ST_LSM6DSRX_LP_MODE, + ST_LSM6DSRX_NO_MODE, +}; + +enum st_lsm6dsrx_fsm_mlc_enable_id { + ST_LSM6DSRX_MLC_FSM_DISABLED = 0, + ST_LSM6DSRX_MLC_ENABLED = BIT(0), + ST_LSM6DSRX_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_lsm6dsrx_mlc_config_t { + uint8_t mlc_int_addr; + uint8_t mlc_int_mask; + uint8_t fsm_int_addr[2]; + uint8_t fsm_int_mask[2]; + uint8_t mlc_configured; + uint8_t fsm_configured; + uint16_t bin_len; + uint16_t requested_odr; + enum st_lsm6dsrx_fsm_mlc_enable_id status; +}; + +/** + * struct st_lsm6dsrx_reg - Generic sensor register description (addr + mask) + * @addr: Address of register. + * @mask: Bitmask register for proper usage. + */ +struct st_lsm6dsrx_reg { + u8 addr; + u8 mask; +}; + +/** + * Register list to be saved before a suspend and restored after a kernel + * resume callback. + */ +enum st_lsm6dsrx_suspend_resume_register { + ST_LSM6DSRX_CTRL1_XL_REG = 0, + ST_LSM6DSRX_CTRL2_G_REG, + ST_LSM6DSRX_REG_CTRL3_C_REG, + ST_LSM6DSRX_REG_CTRL4_C_REG, + ST_LSM6DSRX_REG_CTRL5_C_REG, + ST_LSM6DSRX_REG_CTRL10_C_REG, + ST_LSM6DSRX_REG_TAP_CFG0_REG, + ST_LSM6DSRX_REG_INT1_CTRL_REG, + ST_LSM6DSRX_REG_INT2_CTRL_REG, + ST_LSM6DSRX_REG_FIFO_CTRL1_REG, + ST_LSM6DSRX_REG_FIFO_CTRL2_REG, + ST_LSM6DSRX_REG_FIFO_CTRL3_REG, + ST_LSM6DSRX_REG_FIFO_CTRL4_REG, + ST_LSM6DSRX_REG_EMB_FUNC_EN_B_REG, + ST_LSM6DSRX_REG_FSM_INT1_A_REG, + ST_LSM6DSRX_REG_FSM_INT1_B_REG, + ST_LSM6DSRX_REG_MLC_INT1_REG, + ST_LSM6DSRX_REG_FSM_INT2_A_REG, + ST_LSM6DSRX_REG_FSM_INT2_B_REG, + ST_LSM6DSRX_REG_MLC_INT2_REG, + ST_LSM6DSRX_SUSPEND_RESUME_REGS, +}; + +/** + * Define embedded functions register access + * + * FUNC_CFG_ACCESS_0 is default bank + * FUNC_CFG_ACCESS_SHUB_REG Enable access to the sensor hub (I2C master) + * registers. + * FUNC_CFG_ACCESS_FUNC_CFG Enable access to the embedded functions + * configuration registers. + */ +enum st_lsm6dsrx_page_sel_register { + FUNC_CFG_ACCESS_0 = 0, + FUNC_CFG_ACCESS_SHUB_REG, + FUNC_CFG_ACCESS_FUNC_CFG, +}; + +/** + * struct st_lsm6dsrx_suspend_resume_entry - Register value for backup/restore + * @page: Page bank reg map. + * @addr: Address of register. + * @val: Register value. + * @mask: Bitmask register for proper usage. + */ +struct st_lsm6dsrx_suspend_resume_entry { + u8 page; + u8 addr; + u8 val; + u8 mask; +}; + +/** + * struct st_lsm6dsrx_odr - Single ODR entry + * @hz: Most significant part of the sensor ODR (Hz). + * @uhz: Less significant part of the sensor ODR (micro Hz). + * @val: ODR register value. + * @batch_val: Batching ODR register value. + */ +struct st_lsm6dsrx_odr { + u16 hz; + u32 uhz; + u8 val; + u8 batch_val; +}; + +/** + * struct st_lsm6dsrx_odr_table_entry - Sensor ODR table + * @size: Size of ODR table. + * @reg: ODR register. + * @pm: Power mode register. + * @batching_reg: ODR register for batching on fifo. + * @odr_avl: Array of supported ODR value. + */ +struct st_lsm6dsrx_odr_table_entry { + u8 size; + struct st_lsm6dsrx_reg reg; + struct st_lsm6dsrx_reg pm; + struct st_lsm6dsrx_reg batching_reg; + struct st_lsm6dsrx_odr odr_avl[8]; +}; + +/** + * struct st_lsm6dsrx_fs - Full Scale sensor table entry + * @reg: Register description for FS settings. + * @gain: Sensor sensitivity (mdps/LSB, mg/LSB and uC/LSB). + * @val: FS register value. + */ +struct st_lsm6dsrx_fs { + struct st_lsm6dsrx_reg reg; + u32 gain; + u8 val; +}; + +/** + * struct st_lsm6dsrx_fs_table_entry - Full Scale sensor table + * @size: Full Scale sensor table size. + * @fs_avl: Full Scale list entries. + */ +struct st_lsm6dsrx_fs_table_entry { + u8 size; + struct st_lsm6dsrx_fs fs_avl[5]; +}; + +/** + * List of all sensor ID supported by lsm6dsrx + */ +enum st_lsm6dsrx_sensor_id { + ST_LSM6DSRX_ID_GYRO = 0, + ST_LSM6DSRX_ID_ACC, + ST_LSM6DSRX_ID_TEMP, + ST_LSM6DSRX_ID_EXT0, + ST_LSM6DSRX_ID_EXT1, + ST_LSM6DSRX_ID_MLC, + ST_LSM6DSRX_ID_MLC_0, + ST_LSM6DSRX_ID_MLC_1, + ST_LSM6DSRX_ID_MLC_2, + ST_LSM6DSRX_ID_MLC_3, + ST_LSM6DSRX_ID_MLC_4, + ST_LSM6DSRX_ID_MLC_5, + ST_LSM6DSRX_ID_MLC_6, + ST_LSM6DSRX_ID_MLC_7, + ST_LSM6DSRX_ID_FSM_0, + ST_LSM6DSRX_ID_FSM_1, + ST_LSM6DSRX_ID_FSM_2, + ST_LSM6DSRX_ID_FSM_3, + ST_LSM6DSRX_ID_FSM_4, + ST_LSM6DSRX_ID_FSM_5, + ST_LSM6DSRX_ID_FSM_6, + ST_LSM6DSRX_ID_FSM_7, + ST_LSM6DSRX_ID_FSM_8, + ST_LSM6DSRX_ID_FSM_9, + ST_LSM6DSRX_ID_FSM_10, + ST_LSM6DSRX_ID_FSM_11, + ST_LSM6DSRX_ID_FSM_12, + ST_LSM6DSRX_ID_FSM_13, + ST_LSM6DSRX_ID_FSM_14, + ST_LSM6DSRX_ID_FSM_15, + ST_LSM6DSRX_ID_STEP_COUNTER, + ST_LSM6DSRX_ID_STEP_DETECTOR, + ST_LSM6DSRX_ID_SIGN_MOTION, + ST_LSM6DSRX_ID_TILT, + ST_LSM6DSRX_ID_TAP, + ST_LSM6DSRX_ID_DTAP, + ST_LSM6DSRX_ID_FF, + ST_LSM6DSRX_ID_SLPCHG, + ST_LSM6DSRX_ID_WK, + ST_LSM6DSRX_ID_6D, + ST_LSM6DSRX_ID_MAX, +}; + +/** + * The FIFO only sensor list used by buffer + */ +static const enum st_lsm6dsrx_sensor_id st_lsm6dsrx_buffered_sensor_list[] = { + [0] = ST_LSM6DSRX_ID_GYRO, + [1] = ST_LSM6DSRX_ID_ACC, + [2] = ST_LSM6DSRX_ID_TEMP, + [3] = ST_LSM6DSRX_ID_EXT0, + [4] = ST_LSM6DSRX_ID_EXT1, + [5] = ST_LSM6DSRX_ID_STEP_COUNTER, +}; + + +/** + * The mlc only sensor list used by mlc loader + */ +static const enum st_lsm6dsrx_sensor_id st_lsm6dsrx_mlc_sensor_list[] = { + [0] = ST_LSM6DSRX_ID_MLC_0, + [1] = ST_LSM6DSRX_ID_MLC_1, + [2] = ST_LSM6DSRX_ID_MLC_2, + [3] = ST_LSM6DSRX_ID_MLC_3, + [4] = ST_LSM6DSRX_ID_MLC_4, + [5] = ST_LSM6DSRX_ID_MLC_5, + [6] = ST_LSM6DSRX_ID_MLC_6, + [7] = ST_LSM6DSRX_ID_MLC_7, +}; + +/** + * The fsm only sensor list used by mlc loader + */ +static const enum st_lsm6dsrx_sensor_id st_lsm6dsrx_fsm_sensor_list[] = { + [0] = ST_LSM6DSRX_ID_FSM_0, + [1] = ST_LSM6DSRX_ID_FSM_1, + [2] = ST_LSM6DSRX_ID_FSM_2, + [3] = ST_LSM6DSRX_ID_FSM_3, + [4] = ST_LSM6DSRX_ID_FSM_4, + [5] = ST_LSM6DSRX_ID_FSM_5, + [6] = ST_LSM6DSRX_ID_FSM_6, + [7] = ST_LSM6DSRX_ID_FSM_7, + [8] = ST_LSM6DSRX_ID_FSM_8, + [9] = ST_LSM6DSRX_ID_FSM_9, + [10] = ST_LSM6DSRX_ID_FSM_10, + [11] = ST_LSM6DSRX_ID_FSM_11, + [12] = ST_LSM6DSRX_ID_FSM_12, + [13] = ST_LSM6DSRX_ID_FSM_13, + [14] = ST_LSM6DSRX_ID_FSM_14, + [15] = ST_LSM6DSRX_ID_FSM_15, +}; + +/** + * The low power embedded function only sensor list + */ +static const enum st_lsm6dsrx_sensor_id st_lsm6dsrx_embfunc_sensor_list[] = { + [0] = ST_LSM6DSRX_ID_STEP_COUNTER, + [1] = ST_LSM6DSRX_ID_STEP_DETECTOR, + [2] = ST_LSM6DSRX_ID_SIGN_MOTION, + [3] = ST_LSM6DSRX_ID_TILT, +}; + +/** + * The low power event only sensor list + */ +static const enum st_lsm6dsrx_sensor_id st_lsm6dsrx_event_sensor_list[] = { + [0] = ST_LSM6DSRX_ID_TAP, + [1] = ST_LSM6DSRX_ID_DTAP, + [2] = ST_LSM6DSRX_ID_FF, + [3] = ST_LSM6DSRX_ID_SLPCHG, + [4] = ST_LSM6DSRX_ID_WK, + [5] = ST_LSM6DSRX_ID_6D, +}; + +/** + * The low power event triggered only sensor list + */ +static const enum st_lsm6dsrx_sensor_id st_lsm6dsrx_event_trigger_sensor_list[] = { + [0] = ST_LSM6DSRX_ID_WK, + [1] = ST_LSM6DSRX_ID_6D, +}; + +#define ST_LSM6DSRX_ID_ALL_FSM_MLC (BIT_ULL(ST_LSM6DSRX_ID_MLC_0) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_1) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_2) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_3) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_4) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_5) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_6) | \ + BIT_ULL(ST_LSM6DSRX_ID_MLC_7) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_0) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_1) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_2) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_3) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_4) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_5) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_6) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_7) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_8) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_9) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_10) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_11) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_12) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_13) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_14) | \ + BIT_ULL(ST_LSM6DSRX_ID_FSM_15)) + +/* HW devices that can wakeup the target */ +#define ST_LSM6DSRX_WAKE_UP_SENSORS (BIT_ULL(ST_LSM6DSRX_ID_GYRO) | \ + BIT_ULL(ST_LSM6DSRX_ID_ACC) | \ + ST_LSM6DSRX_ID_ALL_FSM_MLC) + +/* this is the minimal ODR for wake-up sensors and dependencies */ +#define ST_LSM6DSRX_MIN_ODR_IN_WAKEUP 26 + +/* enum supported FIFO mode supported */ +enum st_lsm6dsrx_fifo_mode { + ST_LSM6DSRX_FIFO_BYPASS = 0x0, + ST_LSM6DSRX_FIFO_CONT = 0x6, +}; + +/* enum the FIFO SW operative mode */ +enum { + ST_LSM6DSRX_HW_FLUSH, + ST_LSM6DSRX_HW_OPERATIONAL, +}; + +/** + * struct st_lsm6dsrx_ext_dev_info - Descibe SHUB sensor configuration + * @ext_dev_settings: External sensor descriptor entry [SHUB]. + * @ext_dev_i2c_addr: I2C slave address of device connected to master I2C. + */ +struct st_lsm6dsrx_ext_dev_info { + const struct st_lsm6dsrx_ext_dev_settings *ext_dev_settings; + u8 ext_dev_i2c_addr; +}; + +/* list of HW device id supported by the lsm6dsrx driver */ +enum st_lsm6dsrx_hw_id { + ST_LSM6DSR_ID, + ST_LSM6DSRX_ID, + ST_LSM6DSRX_MAX_ID, +}; + +/** + * struct st_lsm6dsrx_settings - ST IMU sensor settings + * @hw_id: Hw id supported by the driver configuration. + * @name: Device name supported by the driver configuration. + * @st_mlc_probe: MLC probe flag, indicate if MLC feature is supported. + * @st_fsm_probe: FSM probe flag, indicate if FSM feature is supported. + */ +struct st_lsm6dsrx_settings { + struct { + enum st_lsm6dsrx_hw_id hw_id; + const char *name; + } id; + bool st_mlc_probe; + bool st_fsm_probe; +}; + +/** + * struct st_lsm6dsrx_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dsrx_hw. + * @ext_dev_info: For sensor hub indicate device info struct. + * @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. + * @decimator: Sensor sample decimator. + * @dec_counter: Sensor sample decimator counter. + * @old_data: Used by Temperature sensor for data continuity. + * @max_watermark: Max supported FIFO watermark level. + * @watermark: Sensor FIFO watermark level. + * @pm: Sensor power mode (HP, LP). + * @last_fifo_timestamp: Timestamp related to last sample stored in FIFO. + * @selftest_status: Report last self test status. + * @min_st: Min self test raw data value. + * @max_st: Max self test raw data value. + * @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. + */ +struct st_lsm6dsrx_sensor { + char name[32]; + enum st_lsm6dsrx_sensor_id id; + struct st_lsm6dsrx_hw *hw; + struct st_lsm6dsrx_ext_dev_info ext_dev_info; + struct iio_trigger *trig; + + int odr; + int uodr; + + union { + struct { + /* data sensors */ + u32 gain; + u32 offset; + u8 decimator; + u8 dec_counter; + __le16 old_data; + u16 max_watermark; + u16 watermark; + enum st_lsm6dsrx_pm_t pm; + s64 last_fifo_timestamp; + + /* self test */ + int8_t selftest_status; + int min_st; + int max_st; + }; + + struct { + /* mlc or fsm sensor */ + uint8_t status_reg; + uint8_t outreg_addr; + enum st_lsm6dsrx_fsm_mlc_enable_id status; + }; + + struct { + /* event sensor, data configuration */ + u32 conf[6]; + }; + }; +}; + +/** + * struct st_lsm6dsrx_hw - ST IMU MEMS hw instance + * @dev: Pointer to instance of struct device (I2C/SPI/I3C). + * @irq: Device interrupt line. + * @regmap: Regmap structure pointer. + * @int_pin: Store the HW interrupt pin (1 or 2) used by sensor. + * @page_lock: Mutex to prevent access to different register page. + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO. + * @fifo_mode: FIFO operating mode supported by the device. + * @state: HW device operational state. + * @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. + * @enable_mask: Enabled sensor bitmask. + * @requested_mask: Sensor requesting bitmask. + * @ext_data_len: Number of i2c slave devices connected to I2C master. + * @ts_delta_ns: Calibrated delta timestamp. + * @ts_offset: Hw timestamp offset. + * @hw_ts: Latest hw timestamp from the sensor. + * @tsample: Sample timestamp. + * @delta_ts: Estimated delta time between two consecutive interrupts. + * @ts: Latest timestamp from irq handler. + * @i2c_master_pu: I2C master line Pull Up configuration. + * @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. + * @odr_table_entry: Sensors ODR table. + * @preload_mlc: MLC/FSM preload flag. + * @iio_devs: Pointers to iio_dev sensor instances. + * @settings: ST IMU sensor settings. + * @fs_table: ST IMU full scale table. + * @odr_table: ST IMU output data rate table. + */ +struct st_lsm6dsrx_hw { + struct device *dev; + int irq; + struct regmap *regmap; + int int_pin; + + struct mutex page_lock; + struct mutex fifo_lock; + enum st_lsm6dsrx_fifo_mode fifo_mode; + unsigned long state; + s64 hw_timestamp_global; + +#if defined(CONFIG_IIO_ST_LSM6DSRX_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_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + + u64 enable_mask; + u64 requested_mask; + u8 ext_data_len; + u64 ts_delta_ns; + s64 ts_offset; + s64 hw_ts; + s64 tsample; + s64 delta_ts; + s64 ts; + u8 i2c_master_pu; + u32 module_id; + struct iio_mount_matrix orientation; + struct regulator *vdd_supply; + struct regulator *vddio_supply; + + struct st_lsm6dsrx_mlc_config_t *mlc_config; + const struct st_lsm6dsrx_odr_table_entry *odr_table_entry; + + bool preload_mlc; + + struct iio_dev *iio_devs[ST_LSM6DSRX_ID_MAX]; + + const struct st_lsm6dsrx_settings *settings; + const struct st_lsm6dsrx_fs_table_entry *fs_table; + const struct st_lsm6dsrx_odr_table_entry *odr_table; +}; + +/** + * struct st_lsm6dsrx_ff_th - Free Fall threshold table + * @mg: Threshold in mg. + * @val: Register value. + */ +struct st_lsm6dsrx_ff_th { + u32 mg; + u8 val; +}; + +/** + * struct st_lsm6dsrx_6D_th - 6D threshold table + * @deg: Threshold in degrees. + * @val: Register value. + */ +struct st_lsm6dsrx_6D_th { + u8 deg; + u8 val; +}; + +extern const struct dev_pm_ops st_lsm6dsrx_pm_ops; + +static inline bool st_lsm6dsrx_is_fifo_enabled(struct st_lsm6dsrx_hw *hw) +{ + return hw->enable_mask & (BIT_ULL(ST_LSM6DSRX_ID_GYRO) | + BIT_ULL(ST_LSM6DSRX_ID_ACC)); +} + +static inline bool st_lsm6dsrx_run_mlc_task(struct st_lsm6dsrx_hw *hw) +{ + return hw->settings->st_mlc_probe || hw->settings->st_fsm_probe; +} + +static inline int __st_lsm6dsrx_write_with_mask(struct st_lsm6dsrx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_LSM6DSRX_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int +st_lsm6dsrx_update_bits_locked(struct st_lsm6dsrx_hw *hw, unsigned int addr, + unsigned int mask, unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dsrx_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +/* use when mask is constant */ +static inline int st_lsm6dsrx_write_with_mask_locked(struct st_lsm6dsrx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + unsigned int val = ST_LSM6DSRX_SHIFT_VAL(data, mask); + int err; + + mutex_lock(&hw->page_lock); + err = regmap_update_bits(hw->regmap, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_read_locked() - Multiple reg read holding page_lock + * + * @hw: ST IMU sensor instance. + * @addr: Sensor register address. + * @val: Buffer register value to read. + * @len: Number of registers to read. + */ +static inline int +st_lsm6dsrx_read_locked(struct st_lsm6dsrx_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_write_locked() - Single reg write holding page_lock + * + * @hw: ST IMU sensor instance. + * @addr: Sensor register address. + * @val: Register value to set. + */ +static inline int +st_lsm6dsrx_write_locked(struct st_lsm6dsrx_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_lsm6dsrx_set_page_access(struct st_lsm6dsrx_hw *hw, + unsigned int val, + unsigned int mask) +{ + return regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR, + mask, + ST_LSM6DSRX_SHIFT_VAL(val, mask)); +} + +/** + * st_lsm6dsrx_read_with_mask() - Single reg read with mask holding page_lock + * + * @hw: ST IMU sensor instance. + * @addr: Sensor register address. + * @mask: Register mask. + * @val: Register value to read. + */ +static inline int st_lsm6dsrx_read_with_mask(struct st_lsm6dsrx_hw *hw, + u8 addr, u8 mask, u8 *val) +{ + u8 data; + int err; + + err = st_lsm6dsrx_read_locked(hw, addr, &data, sizeof(data)); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + + goto out; + } + + *val = (data & mask) >> __ffs(mask); + +out: + return (err < 0) ? err : 0; +} + +int st_lsm6dsrx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap); +int st_lsm6dsrx_sensor_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable); +int st_lsm6dsrx_buffers_setup(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_get_batch_val(struct st_lsm6dsrx_sensor *sensor, int odr, + int uodr, u8 *val); +int st_lsm6dsrx_update_watermark(struct st_lsm6dsrx_sensor *sensor, + u16 watermark); +ssize_t st_lsm6dsrx_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsrx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsrx_get_watermark(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6dsrx_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsrx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); + +int st_lsm6dsrx_suspend_fifo(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_set_fifo_mode(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_fifo_mode fifo_mode); +int st_lsm6dsrx_update_batching(struct iio_dev *iio_dev, bool enable); +int st_lsm6dsrx_of_get_pin(struct st_lsm6dsrx_hw *hw, int *pin); +int st_lsm6dsrx_shub_probe(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_shub_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable); + +#if defined(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) +int st_lsm6dsrx_hwtimesync_init(struct st_lsm6dsrx_hw *hw); +#else /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ +static inline int +st_lsm6dsrx_hwtimesync_init(struct st_lsm6dsrx_hw *hw) +{ + return 0; +} +#endif /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + +int st_lsm6dsrx_mlc_probe(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_mlc_remove(struct device *dev); +int st_lsm6dsrx_mlc_check_status(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_mlc_init_preload(struct st_lsm6dsrx_hw *hw); + +int st_lsm6dsrx_probe_event(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_event_handler(struct st_lsm6dsrx_hw *hw); + +int st_lsm6dsrx_probe_embfunc(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_embfunc_handler_thread(struct st_lsm6dsrx_hw *hw); +int st_lsm6dsrx_step_counter_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable); +#endif /* ST_LSM6DSRX_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_buffer.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_buffer.c new file mode 100644 index 000000000000..3b7b7ea1c28a --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_buffer.c @@ -0,0 +1,684 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +#define ST_LSM6DSRX_SAMPLE_DISCHARD 0x7ffd + +/* Timestamp convergence filter parameters */ +#define ST_LSM6DSRX_EWMA_LEVEL 120 +#define ST_LSM6DSRX_EWMA_DIV 128 + +#define ST_LSM6DSRX_TIMESTAMP_RESET_VALUE 0xaa + +/* FIFO tags */ +enum { + ST_LSM6DSRX_GYRO_TAG = 0x01, + ST_LSM6DSRX_ACC_TAG = 0x02, + ST_LSM6DSRX_TEMP_TAG = 0x03, + ST_LSM6DSRX_TS_TAG = 0x04, + ST_LSM6DSRX_EXT0_TAG = 0x0f, + ST_LSM6DSRX_EXT1_TAG = 0x10, + ST_LSM6DSRX_STEPC_TAG = 0x12, +}; + +/* Default timeout before to re-enable gyro */ +static int lsm6dsrx_delay_gyro = 10; +module_param(lsm6dsrx_delay_gyro, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); +MODULE_PARM_DESC(lsm6dsrx_delay_gyro, "Delay for Gyro arming"); +static bool delayed_enable_gyro; + +static inline s64 st_lsm6dsrx_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LSM6DSRX_EWMA_DIV - weight) * diff, + ST_LSM6DSRX_EWMA_DIV); + + return old + incr; +} + +static inline int st_lsm6dsrx_reset_hwts(struct st_lsm6dsrx_hw *hw) +{ + u8 data = ST_LSM6DSRX_TIMESTAMP_RESET_VALUE; + int ret; + + ret = st_lsm6dsrx_write_locked(hw, ST_LSM6DSRX_REG_TIMESTAMP2_ADDR, + data); + if (ret < 0) + return ret; + +#if defined(CONFIG_IIO_ST_LSM6DSRX_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_LSM6DSRX_FAST_KTIME); +#else /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + hw->hw_timestamp_global = (hw->hw_timestamp_global + (1LL << 32)) & + GENMASK_ULL(63, 32); +#endif /* CONFIG_IIO_ST_LSM6DSRX_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_lsm6dsrx_set_fifo_mode(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_fifo_mode fifo_mode) +{ + int err; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSRX_REG_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +static inline int +st_lsm6dsrx_set_sensor_batching_odr(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsrx_hw *hw = sensor->hw; + enum st_lsm6dsrx_sensor_id id = sensor->id; + u8 data = 0; + int err; + + if (enable) { + err = st_lsm6dsrx_get_batch_val(sensor, sensor->odr, + sensor->uodr, &data); + if (err < 0) + return err; + } + + return st_lsm6dsrx_update_bits_locked(hw, + hw->odr_table_entry[id].batching_reg.addr, + hw->odr_table_entry[id].batching_reg.mask, + data); +} + +int st_lsm6dsrx_update_watermark(struct st_lsm6dsrx_sensor *sensor, + u16 watermark) +{ + u16 fifo_watermark = ST_LSM6DSRX_MAX_FIFO_DEPTH, cur_watermark = 0; + struct st_lsm6dsrx_hw *hw = sensor->hw; + struct st_lsm6dsrx_sensor *cur_sensor; + __le16 wdata; + int i, err; + int data = 0; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_buffered_sensor_list); + i++) { + enum st_lsm6dsrx_sensor_id id = + st_lsm6dsrx_buffered_sensor_list[i]; + + if (!hw->iio_devs[id]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[id]); + + if (!(hw->enable_mask & BIT_ULL(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_LSM6DSRX_REG_FIFO_CTRL1_ADDR + 1, &data); + if (err < 0) + goto out; + + fifo_watermark = ((data << 8) & ~ST_LSM6DSRX_REG_FIFO_WTM_MASK) | + (fifo_watermark & ST_LSM6DSRX_REG_FIFO_WTM_MASK); + wdata = cpu_to_le16(fifo_watermark); + + err = regmap_bulk_write(hw->regmap, ST_LSM6DSRX_REG_FIFO_CTRL1_ADDR, + &wdata, sizeof(wdata)); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +static struct +iio_dev *st_lsm6dsrx_get_iiodev_from_tag(struct st_lsm6dsrx_hw *hw, u8 tag) +{ + struct iio_dev *iio_dev; + + switch (tag) { + case ST_LSM6DSRX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_GYRO]; + break; + case ST_LSM6DSRX_ACC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_ACC]; + break; + case ST_LSM6DSRX_TEMP_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_TEMP]; + break; + case ST_LSM6DSRX_EXT0_TAG: + if (hw->enable_mask & BIT_ULL(ST_LSM6DSRX_ID_EXT0)) + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_EXT0]; + else + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_EXT1]; + break; + case ST_LSM6DSRX_EXT1_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_EXT1]; + break; + case ST_LSM6DSRX_STEPC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_STEP_COUNTER]; + break; + default: + iio_dev = NULL; + break; + } + + return iio_dev; +} + +static int st_lsm6dsrx_read_fifo(struct st_lsm6dsrx_hw *hw) +{ + u8 iio_buf[ALIGN(ST_LSM6DSRX_FIFO_SAMPLE_SIZE, sizeof(s64)) + + sizeof(s64) + sizeof(s64)]; + u8 buf[6 * ST_LSM6DSRX_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + struct st_lsm6dsrx_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; + + /* return if FIFO is already disabled */ + if (hw->fifo_mode == ST_LSM6DSRX_FIFO_BYPASS) + return 0; + + ts_irq = hw->ts - hw->delta_ts; + + err = st_lsm6dsrx_read_locked(hw, ST_LSM6DSRX_REG_FIFO_STATUS1_ADDR, + &fifo_status, sizeof(fifo_status)); + if (err < 0) + return err; + + fifo_depth = le16_to_cpu(fifo_status) & + ST_LSM6DSRX_REG_FIFO_STATUS_DIFF; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_LSM6DSRX_FIFO_SAMPLE_SIZE; + read_len = 0; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_lsm6dsrx_read_locked(hw, + ST_LSM6DSRX_REG_FIFO_DATA_OUT_TAG_ADDR, + buf, word_len); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_LSM6DSRX_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_LSM6DSRX_TAG_SIZE]; + tag = buf[i] >> 3; + + if (tag == ST_LSM6DSRX_TS_TAG) { + val = get_unaligned_le32(ptr); + +#if defined(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSRX_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_LSM6DSRX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + + hw_ts_old = hw->hw_ts; + hw->hw_ts = val * hw->ts_delta_ns; + hw->ts_offset = st_lsm6dsrx_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_LSM6DSRX_EWMA_LEVEL); + 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_lsm6dsrx_get_iiodev_from_tag(hw, + tag); + if (!iio_dev) + continue; + + sensor = iio_priv(iio_dev); + + /* Skip samples if not ready */ + drdymask = (s16)le16_to_cpu(get_unaligned_le16(ptr)); + if (unlikely(drdymask >= + ST_LSM6DSRX_SAMPLE_DISCHARD)) { + continue; + } + + /* + * hw ts in not queued in FIFO if only step + * counter enabled + */ + if (sensor->id == ST_LSM6DSRX_ID_STEP_COUNTER) { + val = get_unaligned_le32(ptr + 2); + hw->tsample = val * hw->ts_delta_ns; + } + memcpy(iio_buf, ptr, ST_LSM6DSRX_SAMPLE_SIZE); + +#if defined(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + + hw_timestamp_push = cpu_to_le64(hw->hw_timestamp_global); + +#if defined(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + + memcpy(&iio_buf[ALIGN(ST_LSM6DSRX_SAMPLE_SIZE, sizeof(s64))], + &hw_timestamp_push, sizeof(hw_timestamp_push)); + hw->tsample = min_t(s64, + iio_get_time_ns(hw->iio_devs[0]), + hw->tsample); + + sensor->last_fifo_timestamp = hw_timestamp_push; + + /* support decimation for ODR < 12.5 Hz */ + if (sensor->dec_counter > 0) { + sensor->dec_counter--; + } else { + sensor->dec_counter = sensor->decimator; + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + hw->tsample); + } + } + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lsm6dsrx_get_max_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +ssize_t st_lsm6dsrx_get_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +ssize_t st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_update_watermark(sensor, val); + if (err < 0) + goto out; + + sensor->watermark = val; + iio_device_release_direct_mode(iio_dev); + +out: + return err < 0 ? err : size; +} + +ssize_t st_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + s64 type; + s64 event; + int count; + s64 fts; + s64 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_LSM6DSRX_HW_FLUSH, &hw->state); + count = st_lsm6dsrx_read_fifo(hw); + sensor->dec_counter = 0; + fts = sensor->last_fifo_timestamp; + mutex_unlock(&hw->fifo_lock); + + type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fts); + + return size; +} + +int st_lsm6dsrx_suspend_fifo(struct st_lsm6dsrx_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + st_lsm6dsrx_read_fifo(hw); + err = st_lsm6dsrx_set_fifo_mode(hw, ST_LSM6DSRX_FIFO_BYPASS); + mutex_unlock(&hw->fifo_lock); + + return err; +} + +int st_lsm6dsrx_update_batching(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + err = st_lsm6dsrx_set_sensor_batching_odr(sensor, enable); + enable_irq(hw->irq); + + return err; +} + +static int st_lsm6dsrx_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + int err; + + if (sensor->id == ST_LSM6DSRX_ID_GYRO && !enable) + delayed_enable_gyro = true; + + if (sensor->id == ST_LSM6DSRX_ID_GYRO && + enable && delayed_enable_gyro) { + delayed_enable_gyro = false; + msleep(lsm6dsrx_delay_gyro); + } + + disable_irq(hw->irq); + +#if defined(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) + hrtimer_cancel(&hw->timesync_timer); + cancel_work_sync(&hw->timesync_work); +#endif /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + + if (sensor->id == ST_LSM6DSRX_ID_EXT0 || + sensor->id == ST_LSM6DSRX_ID_EXT1) { + err = st_lsm6dsrx_shub_set_enable(sensor, enable); + if (err < 0) + goto out; + } else { + if (sensor->id == ST_LSM6DSRX_ID_STEP_COUNTER) { + err = st_lsm6dsrx_step_counter_set_enable(sensor, + enable); + if (err < 0) + goto out; + } else { + err = st_lsm6dsrx_sensor_set_enable(sensor, + enable); + if (err < 0) + goto out; + + err = st_lsm6dsrx_set_sensor_batching_odr(sensor, + enable); + if (err < 0) + goto out; + } + } + + /* + * this is an auxiliary sensor, it need to get batched + * toghether at least with a primary sensor (Acc/Gyro). + */ + if (sensor->id == ST_LSM6DSRX_ID_TEMP) { + if (!(hw->enable_mask & (BIT_ULL(ST_LSM6DSRX_ID_ACC) | + BIT_ULL(ST_LSM6DSRX_ID_GYRO)))) { + struct st_lsm6dsrx_sensor *acc_sensor; + u8 data = 0; + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSRX_ID_ACC]); + if (enable) { + err = st_lsm6dsrx_get_batch_val(acc_sensor, + sensor->odr, + sensor->uodr, + &data); + if (err < 0) + goto out; + } + + err = st_lsm6dsrx_update_bits_locked(hw, + hw->odr_table_entry[ST_LSM6DSRX_ID_ACC].batching_reg.addr, + hw->odr_table_entry[ST_LSM6DSRX_ID_ACC].batching_reg.mask, + data); + if (err < 0) + goto out; + } + } + + err = st_lsm6dsrx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (enable && hw->fifo_mode == ST_LSM6DSRX_FIFO_BYPASS) { + st_lsm6dsrx_reset_hwts(hw); + err = st_lsm6dsrx_set_fifo_mode(hw, ST_LSM6DSRX_FIFO_CONT); + } else if (!hw->enable_mask) { + err = st_lsm6dsrx_set_fifo_mode(hw, ST_LSM6DSRX_FIFO_BYPASS); + } + +#if defined(CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP) + if (hw->fifo_mode != ST_LSM6DSRX_FIFO_BYPASS) { + hrtimer_start(&hw->timesync_timer, + ktime_set(0, 0), + HRTIMER_MODE_REL); + } +#endif /* CONFIG_IIO_ST_LSM6DSRX_ASYNC_HW_TIMESTAMP */ + +out: + enable_irq(hw->irq); + + return err; +} + +static irqreturn_t st_lsm6dsrx_handler_irq(int irq, void *private) +{ + struct st_lsm6dsrx_hw *hw = (struct st_lsm6dsrx_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_lsm6dsrx_handler_thread(int irq, void *private) +{ + struct st_lsm6dsrx_hw *hw = (struct st_lsm6dsrx_hw *)private; + + if (st_lsm6dsrx_run_mlc_task(hw)) + st_lsm6dsrx_mlc_check_status(hw); + + mutex_lock(&hw->fifo_lock); + st_lsm6dsrx_read_fifo(hw); + clear_bit(ST_LSM6DSRX_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + + st_lsm6dsrx_event_handler(hw); + st_lsm6dsrx_embfunc_handler_thread(hw); + + return IRQ_HANDLED; +} + +static int st_lsm6dsrx_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsrx_update_fifo(iio_dev, true); +} + +static int st_lsm6dsrx_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsrx_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsrx_fifo_ops = { + .preenable = st_lsm6dsrx_fifo_preenable, + .postdisable = st_lsm6dsrx_fifo_postdisable, +}; + +int st_lsm6dsrx_buffers_setup(struct st_lsm6dsrx_hw *hw) +{ + struct device_node *np = hw->dev->of_node; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + 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 = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_CTRL3_C_ADDR, + ST_LSM6DSRX_REG_H_LACTIVE_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_H_LACTIVE_MASK, + irq_active_low)); + if (err < 0) + return err; + + if (np && of_property_read_bool(np, "drive-open-drain")) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_CTRL3_C_ADDR, + ST_LSM6DSRX_REG_PP_OD_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_PP_OD_MASK, 1)); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lsm6dsrx_handler_irq, + st_lsm6dsrx_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_lsm6dsrx_buffered_sensor_list); + i++) { + enum st_lsm6dsrx_sensor_id id = st_lsm6dsrx_buffered_sensor_list[i]; + + if (!hw->iio_devs[id]) + continue; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[id], + INDIO_BUFFER_SOFTWARE, + &st_lsm6dsrx_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[id], buffer); + hw->iio_devs[id]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[id]->setup_ops = &st_lsm6dsrx_fifo_ops; +#endif /* LINUX_VERSION_CODE */ + + } + + err = st_lsm6dsrx_hwtimesync_init(hw); + if (err) + return err; + + return regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSRX_REG_DEC_TS_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_DEC_TS_MASK, 1)); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_core.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_core.c new file mode 100644 index 000000000000..0832e570347e --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_core.c @@ -0,0 +1,1986 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsrx.h" + +static struct st_lsm6dsrx_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 gyro_mask; +} st_lsm6dsrx_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DSRX_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_LSM6DSRX_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DSRX_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DSRX_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DSRX_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DSRX_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +static struct st_lsm6dsrx_power_mode_table { + char *string_mode; + enum st_lsm6dsrx_pm_t val; +} st_lsm6dsrx_power_mode[] = { + [0] = { + .string_mode = "HP_MODE", + .val = ST_LSM6DSRX_HP_MODE, + }, + [1] = { + .string_mode = "LP_MODE", + .val = ST_LSM6DSRX_LP_MODE, + }, +}; + +static struct st_lsm6dsrx_suspend_resume_entry + st_lsm6dsrx_suspend_resume[ST_LSM6DSRX_SUSPEND_RESUME_REGS] = { + [ST_LSM6DSRX_CTRL1_XL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_LSM6DSRX_CTRL2_G_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(3, 2), + }, + [ST_LSM6DSRX_REG_CTRL3_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_CTRL3_C_ADDR, + .mask = ST_LSM6DSRX_REG_BDU_MASK | + ST_LSM6DSRX_REG_PP_OD_MASK | + ST_LSM6DSRX_REG_H_LACTIVE_MASK, + }, + [ST_LSM6DSRX_REG_CTRL4_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_CTRL4_C_ADDR, + .mask = ST_LSM6DSRX_REG_DRDY_MASK, + }, + [ST_LSM6DSRX_REG_CTRL5_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_CTRL5_C_ADDR, + .mask = ST_LSM6DSRX_REG_ROUNDING_MASK, + }, + [ST_LSM6DSRX_REG_CTRL10_C_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_CTRL10_C_ADDR, + .mask = ST_LSM6DSRX_REG_TIMESTAMP_EN_MASK, + }, + [ST_LSM6DSRX_REG_TAP_CFG0_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_TAP_CFG0_ADDR, + .mask = ST_LSM6DSRX_REG_LIR_MASK, + }, + [ST_LSM6DSRX_REG_INT1_CTRL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_INT1_CTRL_ADDR, + .mask = ST_LSM6DSRX_REG_FIFO_TH_MASK, + }, + [ST_LSM6DSRX_REG_INT2_CTRL_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_INT2_CTRL_ADDR, + .mask = ST_LSM6DSRX_REG_FIFO_TH_MASK, + }, + [ST_LSM6DSRX_REG_FIFO_CTRL1_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_FIFO_CTRL1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSRX_REG_FIFO_CTRL2_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_FIFO_CTRL2_ADDR, + .mask = ST_LSM6DSRX_REG_FIFO_WTM8_MASK, + }, + [ST_LSM6DSRX_REG_FIFO_CTRL3_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_FIFO_CTRL3_ADDR, + .mask = ST_LSM6DSRX_REG_BDR_XL_MASK | + ST_LSM6DSRX_REG_BDR_GY_MASK, + }, + [ST_LSM6DSRX_REG_FIFO_CTRL4_REG] = { + .page = FUNC_CFG_ACCESS_0, + .addr = ST_LSM6DSRX_REG_FIFO_CTRL4_ADDR, + .mask = ST_LSM6DSRX_REG_DEC_TS_MASK | + ST_LSM6DSRX_REG_ODR_T_BATCH_MASK, + }, + [ST_LSM6DSRX_REG_EMB_FUNC_EN_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_EMB_FUNC_EN_B_ADDR, + .mask = ST_LSM6DSRX_FSM_EN_MASK | + ST_LSM6DSRX_MLC_EN_MASK, + }, + [ST_LSM6DSRX_REG_FSM_INT1_A_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_FSM_INT1_A_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSRX_REG_FSM_INT1_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_FSM_INT1_B_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSRX_REG_MLC_INT1_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_MLC_INT1_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSRX_REG_FSM_INT2_A_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_FSM_INT2_A_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSRX_REG_FSM_INT2_B_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_FSM_INT2_B_ADDR, + .mask = GENMASK(7, 0), + }, + [ST_LSM6DSRX_REG_MLC_INT2_REG] = { + .page = FUNC_CFG_ACCESS_FUNC_CFG, + .addr = ST_LSM6DSRX_MLC_INT2_ADDR, + .mask = GENMASK(7, 0), + }, +}; + +static const struct st_lsm6dsrx_odr_table_entry st_lsm6dsrx_odr_table[] = { + [ST_LSM6DSRX_ID_ACC] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSRX_CTRL1_XL_ADDR, + .mask = GENMASK(7, 4), + }, + .pm = { + .addr = ST_LSM6DSRX_REG_CTRL6_C_ADDR, + .mask = ST_LSM6DSRX_REG_XL_HM_MODE_MASK, + }, + .batching_reg = { + .addr = ST_LSM6DSRX_REG_FIFO_CTRL3_ADDR, + .mask = GENMASK(3, 0), + }, + .odr_avl[0] = { 1, 600000, 0x01, 0x0b }, + .odr_avl[1] = { 12, 500000, 0x01, 0x01 }, + .odr_avl[2] = { 26, 0, 0x02, 0x02 }, + .odr_avl[3] = { 52, 0, 0x03, 0x03 }, + .odr_avl[4] = { 104, 0, 0x04, 0x04 }, + .odr_avl[5] = { 208, 0, 0x05, 0x05 }, + .odr_avl[6] = { 416, 0, 0x06, 0x06 }, + .odr_avl[7] = { 833, 0, 0x07, 0x07 }, + }, + [ST_LSM6DSRX_ID_GYRO] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(7, 4), + }, + .pm = { + .addr = ST_LSM6DSRX_REG_CTRL7_G_ADDR, + .mask = ST_LSM6DSRX_REG_G_HM_MODE_MASK, + }, + .batching_reg = { + .addr = ST_LSM6DSRX_REG_FIFO_CTRL3_ADDR, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 6, 500000, 0x01, 0x0b }, + .odr_avl[1] = { 12, 500000, 0x01, 0x01 }, + .odr_avl[2] = { 26, 0, 0x02, 0x02 }, + .odr_avl[3] = { 52, 0, 0x03, 0x03 }, + .odr_avl[4] = { 104, 0, 0x04, 0x04 }, + .odr_avl[5] = { 208, 0, 0x05, 0x05 }, + .odr_avl[6] = { 416, 0, 0x06, 0x06 }, + .odr_avl[7] = { 833, 0, 0x07, 0x07 }, + }, + [ST_LSM6DSRX_ID_TEMP] = { + .size = 3, + .batching_reg = { + .addr = ST_LSM6DSRX_REG_FIFO_CTRL4_ADDR, + .mask = GENMASK(5, 4), + }, + .odr_avl[0] = { 1, 600000, 0x01, 0x01 }, + .odr_avl[1] = { 12, 500000, 0x02, 0x02 }, + .odr_avl[2] = { 52, 0, 0x03, 0x03 }, + }, +}; + +/** + * List of supported device settings + * + * The following table list all device features in terms of supported + * MLC/FSM. + */ +static const struct st_lsm6dsrx_settings st_lsm6dsrx_sensor_settings[] = { + { + .id = { + .hw_id = ST_LSM6DSRX_ID, + .name = ST_LSM6DSRX_DEV_NAME, + }, + .st_mlc_probe = true, + .st_fsm_probe = true, + }, + { + .id = { + .hw_id = ST_LSM6DSR_ID, + .name = ST_LSM6DSR_DEV_NAME, + }, + .st_fsm_probe = true, + }, +}; + +static const struct st_lsm6dsrx_fs_table_entry st_lsm6dsrx_fs_table[] = { + [ST_LSM6DSRX_ID_ACC] = { + .size = 4, + .fs_avl[0] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = IIO_G_TO_M_S_2(61000), + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = IIO_G_TO_M_S_2(122000), + .val = 0x2, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = IIO_G_TO_M_S_2(244000), + .val = 0x3, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL1_XL_ADDR, + .mask = GENMASK(3, 2), + }, + .gain = IIO_G_TO_M_S_2(488000), + .val = 0x1, + }, + }, + [ST_LSM6DSRX_ID_GYRO] = { + .size = 5, + .fs_avl[0] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = IIO_DEGREE_TO_RAD(8750000), + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = IIO_DEGREE_TO_RAD(17500000), + .val = 0x4, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = IIO_DEGREE_TO_RAD(35000000), + .val = 0x8, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = IIO_DEGREE_TO_RAD(70000000), + .val = 0x0C, + }, + .fs_avl[4] = { + .reg = { + .addr = ST_LSM6DSRX_CTRL2_G_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = IIO_DEGREE_TO_RAD(140000000), + .val = 0x01, + }, + }, + [ST_LSM6DSRX_ID_TEMP] = { + .size = 1, + .fs_avl[0] = { + .reg = { 0 }, + .gain = (1000000 / ST_LSM6DSRX_TEMP_GAIN), + .val = 0x0 + }, + }, +}; + +static const struct iio_mount_matrix * +st_lsm6dsrx_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *ch) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct iio_chan_spec_ext_info st_lsm6dsrx_chan_spec_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, st_lsm6dsrx_get_mount_matrix), + { } +}; + +#define IIO_CHAN_HW_TIMESTAMP(si) { \ + .type = IIO_COUNT, \ + .address = ST_LSM6DSRX_REG_TIMESTAMP0_ADDR, \ + .scan_index = si, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 64, \ + .storagebits = 64, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_lsm6dsrx_acc_channels[] = { + ST_LSM6DSRX_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSRX_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsrx_chan_spec_ext_info), + ST_LSM6DSRX_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSRX_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsrx_chan_spec_ext_info), + ST_LSM6DSRX_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSRX_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsrx_chan_spec_ext_info), + ST_LSM6DSRX_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsrx_gyro_channels[] = { + ST_LSM6DSRX_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSRX_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsrx_chan_spec_ext_info), + ST_LSM6DSRX_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSRX_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsrx_chan_spec_ext_info), + ST_LSM6DSRX_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSRX_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsrx_chan_spec_ext_info), + ST_LSM6DSRX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsrx_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LSM6DSRX_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_LSM6DSRX_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_HW_TIMESTAMP(1), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +static __maybe_unused int st_lsm6dsrx_reg_access(struct iio_dev *iio_dev, + unsigned int reg, unsigned int writeval, + unsigned int *readval) +{ + struct st_lsm6dsrx_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_lsm6dsrx_set_page_0(struct st_lsm6dsrx_hw *hw) +{ + return regmap_write(hw->regmap, + ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR, 0); +} + +static int st_lsm6dsrx_check_whoami(struct st_lsm6dsrx_hw *hw, + int id) +{ + int err, i; + int data; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_sensor_settings); i++) { + if (st_lsm6dsrx_sensor_settings[i].id.name && + st_lsm6dsrx_sensor_settings[i].id.hw_id == id) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsrx_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_LSM6DSRX_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_LSM6DSRX_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + hw->settings = &st_lsm6dsrx_sensor_settings[i]; + hw->fs_table = st_lsm6dsrx_fs_table; + hw->odr_table = st_lsm6dsrx_odr_table; + + return 0; +} + +static int st_lsm6dsrx_get_odr_calibration(struct st_lsm6dsrx_hw *hw) +{ + int err; + int data; + s64 odr_calib; + + err = regmap_read(hw->regmap, ST_LSM6DSRX_INTERNAL_FREQ_FINE, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %d register\n", + ST_LSM6DSRX_INTERNAL_FREQ_FINE); + return err; + } + + odr_calib = ((s8)data * 37500) / 1000; + hw->ts_delta_ns = ST_LSM6DSRX_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_lsm6dsrx_set_full_scale(struct st_lsm6dsrx_sensor *sensor, + u32 gain) +{ + enum st_lsm6dsrx_sensor_id id = sensor->id; + struct st_lsm6dsrx_hw *hw = sensor->hw; + int i, err; + u8 val; + + for (i = 0; i < st_lsm6dsrx_fs_table[id].size; i++) + if (st_lsm6dsrx_fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == st_lsm6dsrx_fs_table[id].size) + return -EINVAL; + + val = st_lsm6dsrx_fs_table[id].fs_avl[i].val; + err = regmap_update_bits(hw->regmap, + st_lsm6dsrx_fs_table[id].fs_avl[i].reg.addr, + st_lsm6dsrx_fs_table[id].fs_avl[i].reg.mask, + ST_LSM6DSRX_SHIFT_VAL(val, + st_lsm6dsrx_fs_table[id].fs_avl[i].reg.mask)); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dsrx_get_odr_val(enum st_lsm6dsrx_sensor_id id, int odr, + int uodr, struct st_lsm6dsrx_odr *oe) +{ + int req_odr = ST_LSM6DSRX_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < st_lsm6dsrx_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSRX_ODR_EXPAND( + st_lsm6dsrx_odr_table[id].odr_avl[i].hz, + st_lsm6dsrx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= req_odr) { + oe->hz = st_lsm6dsrx_odr_table[id].odr_avl[i].hz; + oe->uhz = st_lsm6dsrx_odr_table[id].odr_avl[i].uhz; + oe->val = st_lsm6dsrx_odr_table[id].odr_avl[i].val; + + return 0; + } + } + + return -EINVAL; +} + +int st_lsm6dsrx_get_batch_val(struct st_lsm6dsrx_sensor *sensor, int odr, + int uodr, u8 *val) +{ + enum st_lsm6dsrx_sensor_id id = sensor->id; + int req_odr = ST_LSM6DSRX_ODR_EXPAND(odr, uodr); + int i; + int sensor_odr; + + for (i = 0; i < st_lsm6dsrx_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSRX_ODR_EXPAND( + st_lsm6dsrx_odr_table[id].odr_avl[i].hz, + st_lsm6dsrx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= req_odr) + break; + } + + if (i == st_lsm6dsrx_odr_table[id].size) + return -EINVAL; + + *val = st_lsm6dsrx_odr_table[id].odr_avl[i].batch_val; + + return 0; +} + +static u16 st_lsm6dsrx_check_odr_dependency(struct st_lsm6dsrx_hw *hw, + int odr, int uodr, + enum st_lsm6dsrx_sensor_id ref_id) +{ + struct st_lsm6dsrx_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = odr > 0; + u16 ret; + + if (enable) { + /* uodr not used */ + if (hw->enable_mask & BIT_ULL(ref_id)) + ret = max_t(u16, ref->odr, odr); + else + ret = odr; + } else { + ret = (hw->enable_mask & BIT_ULL(ref_id)) ? + ref->odr : 0; + } + + return ret; +} + +static int st_lsm6dsrx_set_odr(struct st_lsm6dsrx_sensor *sensor, int req_odr, + int req_uodr) +{ + enum st_lsm6dsrx_sensor_id id = sensor->id; + struct st_lsm6dsrx_hw *hw = sensor->hw; + struct st_lsm6dsrx_odr oe = { 0 }; + + switch (id) { + case ST_LSM6DSRX_ID_EXT0: + case ST_LSM6DSRX_ID_EXT1: + case ST_LSM6DSRX_ID_TEMP: + case ST_LSM6DSRX_ID_FSM_0: + case ST_LSM6DSRX_ID_FSM_1: + case ST_LSM6DSRX_ID_FSM_2: + case ST_LSM6DSRX_ID_FSM_3: + case ST_LSM6DSRX_ID_FSM_4: + case ST_LSM6DSRX_ID_FSM_5: + case ST_LSM6DSRX_ID_FSM_6: + case ST_LSM6DSRX_ID_FSM_7: + case ST_LSM6DSRX_ID_FSM_8: + case ST_LSM6DSRX_ID_FSM_9: + case ST_LSM6DSRX_ID_FSM_10: + case ST_LSM6DSRX_ID_FSM_11: + case ST_LSM6DSRX_ID_FSM_12: + case ST_LSM6DSRX_ID_FSM_13: + case ST_LSM6DSRX_ID_FSM_14: + case ST_LSM6DSRX_ID_FSM_15: + case ST_LSM6DSRX_ID_MLC_0: + case ST_LSM6DSRX_ID_MLC_1: + case ST_LSM6DSRX_ID_MLC_2: + case ST_LSM6DSRX_ID_MLC_3: + case ST_LSM6DSRX_ID_MLC_4: + case ST_LSM6DSRX_ID_MLC_5: + case ST_LSM6DSRX_ID_MLC_6: + case ST_LSM6DSRX_ID_MLC_7: + case ST_LSM6DSRX_ID_STEP_COUNTER: + case ST_LSM6DSRX_ID_STEP_DETECTOR: + case ST_LSM6DSRX_ID_SIGN_MOTION: + case ST_LSM6DSRX_ID_TILT: + case ST_LSM6DSRX_ID_TAP: + case ST_LSM6DSRX_ID_DTAP: + case ST_LSM6DSRX_ID_WK: + case ST_LSM6DSRX_ID_FF: + case ST_LSM6DSRX_ID_SLPCHG: + case ST_LSM6DSRX_ID_6D: + case ST_LSM6DSRX_ID_ACC: { + int odr; + int i; + + id = ST_LSM6DSRX_ID_ACC; + for (i = ST_LSM6DSRX_ID_ACC; i < ST_LSM6DSRX_ID_MAX; i++) { + if (!hw->iio_devs[i] || i == sensor->id) + continue; + + odr = st_lsm6dsrx_check_odr_dependency(hw, req_odr, + req_uodr, i); + if (odr != req_odr) { + /* device already configured */ + return 0; + } + } + break; + } + default: + break; + } + + if (ST_LSM6DSRX_ODR_EXPAND(req_odr, req_uodr) > 0) { + int err; + + err = st_lsm6dsrx_get_odr_val(id, req_odr, req_uodr, &oe); + if (err) + return err; + + /* check if sensor supports power mode setting */ + if (sensor->pm != ST_LSM6DSRX_NO_MODE) { + err = st_lsm6dsrx_update_bits_locked(hw, + st_lsm6dsrx_odr_table[id].pm.addr, + st_lsm6dsrx_odr_table[id].pm.mask, + sensor->pm); + if (err < 0) + return err; + } + } + + return st_lsm6dsrx_update_bits_locked(hw, + st_lsm6dsrx_odr_table[id].reg.addr, + st_lsm6dsrx_odr_table[id].reg.mask, + oe.val); +} + +int st_lsm6dsrx_sensor_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsrx_set_odr(sensor, odr, uodr); + if (err < 0) + return err; + + if (enable) + sensor->hw->enable_mask |= BIT_ULL(sensor->id); + else + sensor->hw->enable_mask &= ~BIT_ULL(sensor->id); + + return 0; +} + +static int st_lsm6dsrx_read_oneshot(struct st_lsm6dsrx_sensor *sensor, + u8 addr, int *val) +{ + struct st_lsm6dsrx_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + if (sensor->id == ST_LSM6DSRX_ID_TEMP) { + u8 status; + + err = st_lsm6dsrx_read_locked(hw, + ST_LSM6DSRX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (err < 0) + return err; + + if (status & ST_LSM6DSRX_REG_STATUS_TDA) { + err = st_lsm6dsrx_read_locked(hw, addr, + &data, sizeof(data)); + if (err < 0) + return err; + + sensor->old_data = data; + } else { + data = sensor->old_data; + } + } else { + err = st_lsm6dsrx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + /* + * - use big delay for data valid because of drdy mask enabled + * - uodr is neglected in this operation + */ + delay = 10000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsrx_read_locked(hw, addr, + &data, sizeof(data)); + + st_lsm6dsrx_sensor_set_enable(sensor, false); + if (err < 0) + return err; + } + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dsrx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsrx_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_lsm6dsrx_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = sensor->offset; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + 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_LSM6DSRX_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; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lsm6dsrx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsrx_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + struct st_lsm6dsrx_odr oe = { 0 }; + + err = st_lsm6dsrx_get_odr_val(sensor->id, val, val2, &oe); + if (!err) { + sensor->odr = oe.hz; + sensor->uodr = oe.uhz; + + /* + * VTS test testSamplingRateHotSwitchOperation not + * toggle the enable status of sensor after changing + * the ODR -> force it + */ + if (sensor->hw->enable_mask & BIT_ULL(sensor->id)) { + switch (sensor->id) { + case ST_LSM6DSRX_ID_GYRO: + case ST_LSM6DSRX_ID_ACC: { + err = st_lsm6dsrx_set_odr(sensor, + sensor->odr, + sensor->uodr); + if (err < 0) + break; + + st_lsm6dsrx_update_batching(iio_dev, 1); + } + break; + default: + break; + } + } + } + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_lsm6dsrx_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsrx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dsrx_odr_table[id].size; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lsm6dsrx_odr_table[id].odr_avl[i].hz, + st_lsm6dsrx_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsrx_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsrx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dsrx_fs_table[id].size; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_lsm6dsrx_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lsm6dsrx_sysfs_get_power_mode_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_power_mode); i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%s ", + st_lsm6dsrx_power_mode[i].string_mode); + } + + buf[len - 1] = '\n'; + + return len; +} + +ssize_t st_lsm6dsrx_get_power_mode(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%s\n", + st_lsm6dsrx_power_mode[sensor->pm].string_mode); +} + +ssize_t st_lsm6dsrx_set_power_mode(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + int err, i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_power_mode); i++) { + if (strncmp(buf, st_lsm6dsrx_power_mode[i].string_mode, + strlen(st_lsm6dsrx_power_mode[i].string_mode)) == 0) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsrx_power_mode)) + return -EINVAL; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + /* update power mode */ + sensor->pm = st_lsm6dsrx_power_mode[i].val; + + iio_device_release_direct_mode(iio_dev); + + return size; +} + +static int __maybe_unused st_lsm6dsrx_bk_regs(struct st_lsm6dsrx_hw *hw) +{ + unsigned int data; + bool restore = 0; + int i, err = 0; + + mutex_lock(&hw->page_lock); + + for (i = 0; i < ST_LSM6DSRX_SUSPEND_RESUME_REGS; i++) { + if (st_lsm6dsrx_suspend_resume[i].page != FUNC_CFG_ACCESS_0) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSRX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_ACCESS_MASK, + st_lsm6dsrx_suspend_resume[i].page)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsrx_suspend_resume[i].addr); + break; + } + + restore = 1; + } + + err = regmap_read(hw->regmap, + st_lsm6dsrx_suspend_resume[i].addr, + &data); + if (err < 0) { + dev_err(hw->dev, + "failed to save register %02x\n", + st_lsm6dsrx_suspend_resume[i].addr); + goto out_lock; + } + + if (restore) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSRX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_ACCESS_MASK, + FUNC_CFG_ACCESS_0)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsrx_suspend_resume[i].addr); + break; + } + + restore = 0; + } + + st_lsm6dsrx_suspend_resume[i].val = data; + } + +out_lock: + mutex_unlock(&hw->page_lock); + + return err; +} + +static int __maybe_unused st_lsm6dsrx_restore_regs(struct st_lsm6dsrx_hw *hw) +{ + bool restore = 0; + int i, err = 0; + + mutex_lock(&hw->page_lock); + + for (i = 0; i < ST_LSM6DSRX_SUSPEND_RESUME_REGS; i++) { + if (st_lsm6dsrx_suspend_resume[i].page != FUNC_CFG_ACCESS_0) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSRX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_ACCESS_MASK, + st_lsm6dsrx_suspend_resume[i].page)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsrx_suspend_resume[i].addr); + break; + } + + restore = 1; + } + + err = regmap_update_bits(hw->regmap, + st_lsm6dsrx_suspend_resume[i].addr, + st_lsm6dsrx_suspend_resume[i].mask, + st_lsm6dsrx_suspend_resume[i].val); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsrx_suspend_resume[i].addr); + break; + } + + if (restore) { + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DSRX_REG_ACCESS_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_ACCESS_MASK, + FUNC_CFG_ACCESS_0)); + if (err < 0) { + dev_err(hw->dev, + "failed to update %02x reg\n", + st_lsm6dsrx_suspend_resume[i].addr); + break; + } + + restore = 0; + } + } + + mutex_unlock(&hw->page_lock); + + return err; +} + +int st_lsm6dsrx_of_get_pin(struct st_lsm6dsrx_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_lsm6dsrx_get_int_reg(struct st_lsm6dsrx_hw *hw, u8 *drdy_reg) +{ + int err = 0, int_pin; + + if (st_lsm6dsrx_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_LSM6DSRX_REG_INT1_CTRL_ADDR; + break; + case 2: + *drdy_reg = ST_LSM6DSRX_REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported interrupt pin\n"); + err = -EINVAL; + break; + } + + hw->int_pin = int_pin; + + return err; +} + +static int st_lsm6dsrx_set_selftest( + struct st_lsm6dsrx_sensor *sensor, int index) +{ + u8 mode, mask; + + switch (sensor->id) { + case ST_LSM6DSRX_ID_ACC: + mask = ST_LSM6DSRX_REG_ST_XL_MASK; + mode = st_lsm6dsrx_selftest_table[index].accel_value; + break; + case ST_LSM6DSRX_ID_GYRO: + mask = ST_LSM6DSRX_REG_ST_G_MASK; + mode = st_lsm6dsrx_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + return st_lsm6dsrx_update_bits_locked(sensor->hw, + ST_LSM6DSRX_REG_CTRL5_C_ADDR, + mask, mode); +} + +static ssize_t st_lsm6dsrx_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6dsrx_selftest_table[1].string_mode, + st_lsm6dsrx_selftest_table[2].string_mode); +} + +static ssize_t st_lsm6dsrx_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int8_t result; + char *message = NULL; + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsrx_sensor_id id = sensor->id; + + if (id != ST_LSM6DSRX_ID_ACC && + id != ST_LSM6DSRX_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_lsm6dsrx_selftest_sensor(struct st_lsm6dsrx_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_LSM6DSRX_ID_ACC: + reg = ST_LSM6DSRX_REG_OUTX_L_A_ADDR; + bitmask = ST_LSM6DSRX_REG_STATUS_XLDA; + break; + case ST_LSM6DSRX_ID_GYRO: + reg = ST_LSM6DSRX_REG_OUTX_L_G_ADDR; + bitmask = ST_LSM6DSRX_REG_STATUS_GDA; + break; + default: + return -EINVAL; + } + + /* set selftest normal mode */ + ret = st_lsm6dsrx_set_selftest(sensor, 0); + if (ret < 0) + return ret; + + ret = st_lsm6dsrx_sensor_set_enable(sensor, true); + if (ret < 0) + return ret; + + /* wait at least 2 ODRs to be sure */ + delay = 2 * (1000000 / 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, 2 * delay); + ret = st_lsm6dsrx_read_locked(sensor->hw, + ST_LSM6DSRX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsrx_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 acc samples missing (expected %d, read %d)\n", + i, n); + ret = -1; + + goto selftest_failure; + } + + n = 0; + + /* set selftest mode */ + st_lsm6dsrx_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, 2 * delay); + ret = st_lsm6dsrx_read_locked(sensor->hw, + ST_LSM6DSRX_REG_STATUS_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsrx_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_lsm6dsrx_set_selftest(sensor, 0); + + return st_lsm6dsrx_sensor_set_enable(sensor, false); +} + +static ssize_t st_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + enum st_lsm6dsrx_sensor_id id = sensor->id; + struct st_lsm6dsrx_hw *hw = sensor->hw; + int ret, test; + u8 drdy_reg; + u32 gain; + + if (id != ST_LSM6DSRX_ID_ACC && + id != ST_LSM6DSRX_ID_GYRO) + return -EINVAL; + + for (test = 0; test < ARRAY_SIZE(st_lsm6dsrx_selftest_table); test++) { + if (strncmp(buf, st_lsm6dsrx_selftest_table[test].string_mode, + strlen(st_lsm6dsrx_selftest_table[test].string_mode)) == 0) + break; + } + + if (test == ARRAY_SIZE(st_lsm6dsrx_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_ULL(id)) { + ret = -EBUSY; + + goto out_claim; + } + + st_lsm6dsrx_bk_regs(hw); + + /* disable FIFO watermak interrupt */ + ret = st_lsm6dsrx_get_int_reg(hw, &drdy_reg); + if (ret < 0) + goto restore_regs; + + ret = st_lsm6dsrx_update_bits_locked(hw, drdy_reg, + ST_LSM6DSRX_REG_FIFO_TH_MASK, 0); + if (ret < 0) + goto restore_regs; + + gain = sensor->gain; + if (id == ST_LSM6DSRX_ID_ACC) { + /* set BDU = 1, FS = 4 g, ODR = 52 Hz */ + st_lsm6dsrx_set_full_scale(sensor, IIO_G_TO_M_S_2(122)); + st_lsm6dsrx_set_odr(sensor, 52, 0); + st_lsm6dsrx_selftest_sensor(sensor, test); + + /* restore full scale after test */ + st_lsm6dsrx_set_full_scale(sensor, gain); + } else { + /* set BDU = 1, ODR = 208 Hz, FS = 2000 dps */ + st_lsm6dsrx_set_full_scale(sensor, IIO_DEGREE_TO_RAD(70000)); + st_lsm6dsrx_set_odr(sensor, 208, 0); + st_lsm6dsrx_selftest_sensor(sensor, test); + + /* restore full scale after test */ + st_lsm6dsrx_set_full_scale(sensor, gain); + } + +restore_regs: + st_lsm6dsrx_restore_regs(hw); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +static int st_lsm6dsrx_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (mask == 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 -EINVAL; + } + } + + return -EINVAL; +} + +ssize_t st_lsm6dsrx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsrx_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dsrx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dsrx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_lsm6dsrx_sysfs_scale_avail, NULL, 0); + +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsrx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsrx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsrx_get_watermark, + st_lsm6dsrx_set_watermark, 0); + +static IIO_DEVICE_ATTR(power_mode_available, 0444, + st_lsm6dsrx_sysfs_get_power_mode_avail, NULL, 0); +static IIO_DEVICE_ATTR(power_mode, 0644, + st_lsm6dsrx_get_power_mode, + st_lsm6dsrx_set_power_mode, 0); + +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lsm6dsrx_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_lsm6dsrx_sysfs_get_selftest_status, + st_lsm6dsrx_sysfs_start_selftest, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsrx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsrx_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.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_lsm6dsrx_acc_attribute_group = { + .attrs = st_lsm6dsrx_acc_attributes, +}; + +static const struct iio_info st_lsm6dsrx_acc_info = { + .attrs = &st_lsm6dsrx_acc_attribute_group, + .read_raw = st_lsm6dsrx_read_raw, + .write_raw = st_lsm6dsrx_write_raw, + .write_raw_get_fmt = st_lsm6dsrx_write_raw_get_fmt, +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_lsm6dsrx_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static struct attribute *st_lsm6dsrx_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_power_mode_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + &iio_dev_attr_power_mode.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_lsm6dsrx_gyro_attribute_group = { + .attrs = st_lsm6dsrx_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsrx_gyro_info = { + .attrs = &st_lsm6dsrx_gyro_attribute_group, + .read_raw = st_lsm6dsrx_read_raw, + .write_raw = st_lsm6dsrx_write_raw, + .write_raw_get_fmt = st_lsm6dsrx_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_temp_attribute_group = { + .attrs = st_lsm6dsrx_temp_attributes, +}; + +static const struct iio_info st_lsm6dsrx_temp_info = { + .attrs = &st_lsm6dsrx_temp_attribute_group, + .read_raw = st_lsm6dsrx_read_raw, + .write_raw = st_lsm6dsrx_write_raw, + .write_raw_get_fmt = st_lsm6dsrx_write_raw_get_fmt, +}; + +static const unsigned long st_lsm6dsrx_available_scan_masks[] = { + GENMASK(3, 0), 0x0 +}; + +static const unsigned long st_lsm6dsrx_temp_available_scan_masks[] = { + 0x1, 0x0 +}; + +static int st_lsm6dsrx_reset_device(struct st_lsm6dsrx_hw *hw) +{ + int err; + + /* sw reset */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_CTRL3_C_ADDR, + ST_LSM6DSRX_REG_SW_RESET_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_SW_RESET_MASK, 1)); + if (err < 0) + return err; + + msleep(50); + + /* boot */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_CTRL3_C_ADDR, + ST_LSM6DSRX_REG_BOOT_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_BOOT_MASK, 1)); + + msleep(50); + + return err; +} + +static int st_lsm6dsrx_init_timestamp_engine(struct st_lsm6dsrx_hw *hw, + bool enable) +{ + int err; + + /* init timestamp engine */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_CTRL10_C_ADDR, + ST_LSM6DSRX_REG_TIMESTAMP_EN_MASK, + ST_LSM6DSRX_SHIFT_VAL(true, + ST_LSM6DSRX_REG_TIMESTAMP_EN_MASK)); + if (err < 0) + return err; + + /* enable timestamp rollover interrupt on int2 */ + return regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_MD2_CFG_ADDR, + ST_LSM6DSRX_REG_INT2_TIMESTAMP_MASK, + ST_LSM6DSRX_SHIFT_VAL(enable, + ST_LSM6DSRX_REG_INT2_TIMESTAMP_MASK)); +} + +static int st_lsm6dsrx_init_device(struct st_lsm6dsrx_hw *hw) +{ + u8 drdy_reg; + int err; + + /* latch interrupts */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_TAP_CFG0_ADDR, + ST_LSM6DSRX_REG_LIR_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_LIR_MASK, 1)); + if (err < 0) + return err; + + /* enable (B)lock (D)ata (U)pdate */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_CTRL3_C_ADDR, + ST_LSM6DSRX_REG_BDU_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_BDU_MASK, 1)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_CTRL5_C_ADDR, + ST_LSM6DSRX_REG_ROUNDING_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_ROUNDING_MASK, 3)); + if (err < 0) + return err; + + err = st_lsm6dsrx_init_timestamp_engine(hw, true); + if (err < 0) + return err; + + err = st_lsm6dsrx_get_int_reg(hw, &drdy_reg); + if (err < 0) + return err; + + /* Enable DRDY MASK for filters settling time */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSRX_REG_CTRL4_C_ADDR, + ST_LSM6DSRX_REG_DRDY_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_DRDY_MASK, 1)); + + if (err < 0) + return err; + + /* enable interrupt on FIFO watermak */ + return regmap_update_bits(hw->regmap, drdy_reg, + ST_LSM6DSRX_REG_FIFO_TH_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_FIFO_TH_MASK, 1)); +} + +static struct iio_dev *st_lsm6dsrx_alloc_iiodev(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_sensor_id id) +{ + struct st_lsm6dsrx_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; + + /* set default FS to each sensor */ + sensor->gain = st_lsm6dsrx_fs_table[id].fs_avl[0].gain; + + switch (id) { + case ST_LSM6DSRX_ID_ACC: + iio_dev->channels = st_lsm6dsrx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_accel", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_acc_info; + iio_dev->available_scan_masks = + st_lsm6dsrx_available_scan_masks; + sensor->max_watermark = ST_LSM6DSRX_MAX_FIFO_DEPTH; + sensor->offset = 0; + sensor->pm = ST_LSM6DSRX_HP_MODE; + sensor->odr = st_lsm6dsrx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsrx_odr_table[id].odr_avl[1].uhz; + sensor->min_st = ST_LSM6DSRX_SELFTEST_ACCEL_MIN; + sensor->max_st = ST_LSM6DSRX_SELFTEST_ACCEL_MAX; + break; + case ST_LSM6DSRX_ID_GYRO: + iio_dev->channels = st_lsm6dsrx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_gyro_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_gyro", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_gyro_info; + iio_dev->available_scan_masks = + st_lsm6dsrx_available_scan_masks; + sensor->max_watermark = ST_LSM6DSRX_MAX_FIFO_DEPTH; + sensor->offset = 0; + sensor->pm = ST_LSM6DSRX_HP_MODE; + sensor->odr = st_lsm6dsrx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsrx_odr_table[id].odr_avl[1].uhz; + sensor->min_st = ST_LSM6DSRX_SELFTEST_GYRO_MIN; + sensor->max_st = ST_LSM6DSRX_SELFTEST_GYRO_MAX; + break; + case ST_LSM6DSRX_ID_TEMP: + iio_dev->channels = st_lsm6dsrx_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_temp_info; + iio_dev->available_scan_masks = + st_lsm6dsrx_temp_available_scan_masks; + sensor->max_watermark = ST_LSM6DSRX_MAX_FIFO_DEPTH; + sensor->offset = ST_LSM6DSRX_TEMP_OFFSET; + sensor->pm = ST_LSM6DSRX_NO_MODE; + sensor->odr = st_lsm6dsrx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsrx_odr_table[id].odr_avl[1].uhz; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static void st_lsm6dsrx_get_properties(struct st_lsm6dsrx_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +static void st_lsm6dsrx_disable_regulator_action(void *_data) +{ + struct st_lsm6dsrx_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static int st_lsm6dsrx_power_enable(struct st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap) +{ + struct st_lsm6dsrx_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->fifo_lock); + mutex_init(&hw->page_lock); + + hw->regmap = regmap; + hw->dev = dev; + hw->irq = irq; + hw->odr_table_entry = st_lsm6dsrx_odr_table; + + err = st_lsm6dsrx_power_enable(hw); + if (err != 0) + return err; + + err = st_lsm6dsrx_set_page_0(hw); + if (err < 0) + return err; + + err = st_lsm6dsrx_check_whoami(hw, hw_id); + if (err < 0) + return err; + + st_lsm6dsrx_get_properties(hw); + + err = st_lsm6dsrx_get_odr_calibration(hw); + if (err < 0) + return err; + + err = st_lsm6dsrx_reset_device(hw); + if (err < 0) + return err; + + err = st_lsm6dsrx_init_device(hw); + if (err < 0) + return err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,15,0) + err = iio_read_mount_matrix(dev, &hw->orientation); +#elif LINUX_VERSION_CODE >= KERNEL_VERSION(5,2,0) + 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; + } + + /* register only main data sensors */ + for (i = 0; i <= ST_LSM6DSRX_ID_TEMP; i++) { + hw->iio_devs[i] = st_lsm6dsrx_alloc_iiodev(hw, i); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + err = st_lsm6dsrx_shub_probe(hw); + if (err < 0) + return err; + + /* allocate step counter before buffer setup because use FIFO */ + err = st_lsm6dsrx_probe_embfunc(hw); + if (err < 0) + return err; + + err = st_lsm6dsrx_probe_event(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_lsm6dsrx_buffers_setup(hw); + if (err < 0) + return err; + } + + if (st_lsm6dsrx_run_mlc_task(hw)) { + err = st_lsm6dsrx_mlc_probe(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LSM6DSRX_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_lsm6dsrx_run_mlc_task(hw)) { + err = st_lsm6dsrx_mlc_init_preload(hw); + if (err) + return err; + } + + device_init_wakeup(dev, + device_property_read_bool(dev, "wakeup-source")); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsrx_probe); + +static int __maybe_unused st_lsm6dsrx_suspend(struct device *dev) +{ + struct st_lsm6dsrx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSRX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT_ULL(sensor->id))) + continue; + + if (device_may_wakeup(dev) && + ((hw->enable_mask & BIT_ULL(sensor->id)) & + ST_LSM6DSRX_WAKE_UP_SENSORS)) { + /* do not disable sensors if requested by wake-up */ + err = st_lsm6dsrx_set_odr(sensor, + ST_LSM6DSRX_MIN_ODR_IN_WAKEUP, + 0); + if (err < 0) + return err; + } else { + err = st_lsm6dsrx_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + } + + if (st_lsm6dsrx_is_fifo_enabled(hw)) { + err = st_lsm6dsrx_suspend_fifo(hw); + if (err < 0) + return err; + } + + err = st_lsm6dsrx_bk_regs(hw); + + if (device_may_wakeup(dev) && + (hw->enable_mask & ST_LSM6DSRX_WAKE_UP_SENSORS)) + enable_irq_wake(hw->irq); + + dev_info(dev, "Suspending device\n"); + + return err < 0 ? err : 0; +} + +static int __maybe_unused st_lsm6dsrx_resume(struct device *dev) +{ + struct st_lsm6dsrx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsrx_sensor *sensor; + int i, err = 0; + + dev_info(dev, "Resuming device\n"); + + if (device_may_wakeup(dev)) + disable_irq_wake(hw->irq); + + err = st_lsm6dsrx_restore_regs(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSRX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (hw->enable_mask & BIT_ULL(sensor->id)) { + err = st_lsm6dsrx_set_odr(sensor, sensor->odr, + sensor->uodr); + if (err < 0) + return err; + } + } + + if (st_lsm6dsrx_is_fifo_enabled(hw)) + err = st_lsm6dsrx_set_fifo_mode(hw, ST_LSM6DSRX_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_lsm6dsrx_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsrx_suspend, st_lsm6dsrx_resume) +}; +EXPORT_SYMBOL(st_lsm6dsrx_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsrx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_embfunc.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_embfunc.c new file mode 100644 index 000000000000..315f2836640c --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_embfunc.c @@ -0,0 +1,568 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx embedded function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.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_lsm6dsrx_step_counter_channels[] = { + { + .type = IIO_STEP_COUNTER, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + ST_LSM6DSRX_EVENT_CHANNEL(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_lsm6dsrx_step_detector_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_STEP_DETECTOR, 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_lsm6dsrx_sign_motion_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(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_lsm6dsrx_tilt_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_TILT, thr), +}; + +static const unsigned long st_lsm6dsrx_embfunc_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int +st_lsm6dsrx_embfunc_set_enable(struct st_lsm6dsrx_sensor *sensor, + u8 mask, u8 irq_mask, bool enable) +{ + struct st_lsm6dsrx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSRX_REG_EMB_FUNC_INT1_ADDR : + ST_LSM6DSRX_REG_EMB_FUNC_INT2_ADDR; + int err; + + err = st_lsm6dsrx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, 1, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock; + + err = __st_lsm6dsrx_write_with_mask(hw, + ST_LSM6DSRX_REG_EMB_FUNC_EN_A_ADDR, + mask, + enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsrx_write_with_mask(hw, int_reg, irq_mask, + enable ? 1 : 0); + +reset_page: + st_lsm6dsrx_set_page_access(hw, 0, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_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_lsm6dsrx_embfunc_sensor_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + int err; + + switch (sensor->id) { + case ST_LSM6DSRX_ID_STEP_DETECTOR: + err = st_lsm6dsrx_embfunc_set_enable(sensor, + ST_LSM6DSRX_REG_PEDO_EN_MASK, + ST_LSM6DSRX_INT_STEP_DETECTOR_MASK, + enable); + break; + case ST_LSM6DSRX_ID_SIGN_MOTION: + err = st_lsm6dsrx_embfunc_set_enable(sensor, + ST_LSM6DSRX_REG_SIGN_MOTION_EN_MASK, + ST_LSM6DSRX_INT_SIG_MOT_MASK, + enable); + break; + case ST_LSM6DSRX_ID_TILT: + err = st_lsm6dsrx_embfunc_set_enable(sensor, + ST_LSM6DSRX_REG_TILT_EN_MASK, + ST_LSM6DSRX_INT_TILT_MASK, + enable); + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +/** + * st_lsm6dsrx_reset_step_counter() - Reset Step Counter value [EMB_FUN] + * + * @iio_dev: IIO device + * + * return < 0 if error, 0 otherwise + */ +static int st_lsm6dsrx_reset_step_counter(struct iio_dev *iio_dev) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, 1, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock_page; + + err = __st_lsm6dsrx_write_with_mask(hw, + ST_LSM6DSRX_REG_EMB_FUNC_SRC_ADDR, + ST_LSM6DSRX_PEDO_RST_STEP_MASK, 1); + st_lsm6dsrx_set_page_access(hw, 0, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + +unlock_page: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT_ULL(sensor->id)); +} + +/** + * st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_lsm6dsrx_embfunc_sensor_set_enable(sensor, state); + iio_device_release_direct_mode(iio_dev); + + return err; +} + +/** + * st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_sysfs_reset_step_counter, 0); + +static IIO_DEVICE_ATTR(hwfifo_stepc_watermark_max, 0444, + st_lsm6dsrx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_stepc_flush, 0200, NULL, + st_lsm6dsrx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_stepc_watermark, 0644, + st_lsm6dsrx_get_watermark, + st_lsm6dsrx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsrx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_step_counter_attribute_group = { + .attrs = st_lsm6dsrx_step_counter_attributes, +}; + +static const struct iio_info st_lsm6dsrx_step_counter_info = { + .attrs = &st_lsm6dsrx_step_counter_attribute_group, +}; + +static struct attribute *st_lsm6dsrx_step_detector_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsrx_step_detector_attribute_group = { + .attrs = st_lsm6dsrx_step_detector_attributes, +}; + +static const struct iio_info st_lsm6dsrx_step_detector_info = { + .attrs = &st_lsm6dsrx_step_detector_attribute_group, + .read_event_config = st_lsm6dsrx_read_embfunc_config, + .write_event_config = st_lsm6dsrx_write_embfunc_config, +}; + +static struct attribute *st_lsm6dsrx_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsrx_sign_motion_attribute_group = { + .attrs = st_lsm6dsrx_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6dsrx_sign_motion_info = { + .attrs = &st_lsm6dsrx_sign_motion_attribute_group, + .read_event_config = st_lsm6dsrx_read_embfunc_config, + .write_event_config = st_lsm6dsrx_write_embfunc_config, +}; + +static struct attribute *st_lsm6dsrx_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsrx_tilt_attribute_group = { + .attrs = st_lsm6dsrx_tilt_attributes, +}; + +static const struct iio_info st_lsm6dsrx_tilt_info = { + .attrs = &st_lsm6dsrx_tilt_attribute_group, + .read_event_config = st_lsm6dsrx_read_embfunc_config, + .write_event_config = st_lsm6dsrx_write_embfunc_config, +}; + +static int st_lsm6dsrx_embfunc_init(struct st_lsm6dsrx_hw *hw) +{ + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSRX_REG_MD1_CFG_ADDR : + ST_LSM6DSRX_REG_MD2_CFG_ADDR; + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, 1, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock_page; + + /* enable embedded function latched interrupt */ + err = __st_lsm6dsrx_write_with_mask(hw, + ST_LSM6DSRX_REG_PAGE_RW_ADDR, + ST_LSM6DSRX_EMB_FUNC_LIR_MASK, 1); + st_lsm6dsrx_set_page_access(hw, 0, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + + /* enable embedded function interrupt by default */ + err = __st_lsm6dsrx_write_with_mask(hw, int_reg, + ST_LSM6DSRX_REG_INT_EMB_FUNC_MASK, + 1); +unlock_page: + mutex_unlock(&hw->page_lock); + + return err; +} + +static struct iio_dev * +st_lsm6dsrx_alloc_embfunc_iiodev(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_sensor_id id) +{ + struct st_lsm6dsrx_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_lsm6dsrx_embfunc_available_scan_masks; + + /* set main sensor odr to 26 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[2].hz; + switch (id) { + case ST_LSM6DSRX_ID_STEP_COUNTER: + iio_dev->channels = st_lsm6dsrx_step_counter_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_step_counter_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_stepc", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_step_counter_info; + break; + case ST_LSM6DSRX_ID_STEP_DETECTOR: + iio_dev->channels = st_lsm6dsrx_step_detector_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_step_detector_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_stepd", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_step_detector_info; + break; + case ST_LSM6DSRX_ID_SIGN_MOTION: + iio_dev->channels = st_lsm6dsrx_sign_motion_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_sign_motion_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sigmot", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_sign_motion_info; + break; + case ST_LSM6DSRX_ID_TILT: + iio_dev->channels = st_lsm6dsrx_tilt_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_tilt_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tilt", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_tilt_info; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/** + * st_lsm6dsrx_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_lsm6dsrx_step_counter_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsrx_hw *hw = sensor->hw; + int err; + + err = st_lsm6dsrx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, 1, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); + if (err < 0) + goto unlock; + + err = __st_lsm6dsrx_write_with_mask(hw, + ST_LSM6DSRX_REG_EMB_FUNC_EN_A_ADDR, + ST_LSM6DSRX_REG_PEDO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + /* enable step counter batching in fifo */ + err = __st_lsm6dsrx_write_with_mask(hw, + ST_LSM6DSRX_REG_EMB_FUNC_FIFO_CFG_ADDR, + ST_LSM6DSRX_PEDO_FIFO_EN_MASK, + enable); + +reset_page: + st_lsm6dsrx_set_page_access(hw, 0, + ST_LSM6DSRX_REG_FUNC_CFG_MASK); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_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_lsm6dsrx_embfunc_handler_thread(struct st_lsm6dsrx_hw *hw) +{ + if (hw->enable_mask & (BIT_ULL(ST_LSM6DSRX_ID_STEP_DETECTOR) | + BIT_ULL(ST_LSM6DSRX_ID_SIGN_MOTION) | + BIT_ULL(ST_LSM6DSRX_ID_TILT))) { + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + err = st_lsm6dsrx_read_locked(hw, + ST_LSM6DSRX_REG_EMB_FUNC_STATUS_MAINPAGE_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* embedded function sensors */ + if (status & ST_LSM6DSRX_IS_STEP_DET_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_STEP_DETECTOR]; + event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, + -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LSM6DSRX_IS_SIGMOT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_SIGN_MOTION]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSRX_IS_TILT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_TILT]; + event = IIO_UNMOD_EVENT_CODE(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_lsm6dsrx_probe_embfunc() - Allocate IIO embedded function device + * + * @hw: ST IMU MEMS hw instance. + * + * return 0 or < 0 for error + */ +int st_lsm6dsrx_probe_embfunc(struct st_lsm6dsrx_hw *hw) +{ + enum st_lsm6dsrx_sensor_id id; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_embfunc_sensor_list); + i++) { + + id = st_lsm6dsrx_embfunc_sensor_list[i]; + hw->iio_devs[id] = st_lsm6dsrx_alloc_embfunc_iiodev(hw, + id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + return st_lsm6dsrx_embfunc_init(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_events.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_events.c new file mode 100644 index 000000000000..20892a740703 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_events.c @@ -0,0 +1,1048 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx events function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +static const struct +st_lsm6dsrx_ff_th st_lsm6dsrx_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_lsm6dsrx_6D_th st_lsm6dsrx_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_lsm6dsrx_event_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static const struct iio_chan_spec st_lsm6dsrx_wk_channels[] = { + { + .type = 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_lsm6dsrx_ff_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_lsm6dsrx_sc_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_lsm6dsrx_6D_channels[] = { + { + .type = 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_lsm6dsrx_tap_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_TAP, thr), +}; + +static const struct iio_chan_spec st_lsm6dsrx_dtap_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_TAP_TAP, thr), +}; + +/* + * st_lsm6dsrx_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_lsm6dsrx_set_wake_up_thershold(struct st_lsm6dsrx_hw *hw, + int th_ug) +{ + struct st_lsm6dsrx_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_lsm6dsrx_read_with_mask(hw, + hw->fs_table[ST_LSM6DSRX_ID_ACC].fs_avl[0].reg.addr, + hw->fs_table[ST_LSM6DSRX_ID_ACC].fs_avl[0].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_LSM6DSRX_WAKE_UP_THS_MASK >> + __ffs(ST_LSM6DSRX_WAKE_UP_THS_MASK); + if (val > max_th) + val = max_th; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSRX_WAKE_UP_THS_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[0] = th_ug; + + return 0; +} + +/* + * st_lsm6dsrx_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_lsm6dsrx_set_wake_up_duration(struct st_lsm6dsrx_hw *hw, + int dur_ms) +{ + struct st_lsm6dsrx_sensor *sensor; + struct iio_dev *iio_dev; + int i, tmp, sensor_odr, err; + u8 val, odr_xl, max_dur; + + err = st_lsm6dsrx_read_with_mask(hw, + hw->odr_table[ST_LSM6DSRX_ID_ACC].reg.addr, + hw->odr_table[ST_LSM6DSRX_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_LSM6DSRX_ID_ACC].odr_avl[2].val; + } + + for (i = 0; i < hw->odr_table[ST_LSM6DSRX_ID_ACC].size; i++) { + if (odr_xl == + hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[i].val) + break; + } + + if (i == hw->odr_table[ST_LSM6DSRX_ID_ACC].size) + return -EINVAL; + + + sensor_odr = ST_LSM6DSRX_ODR_EXPAND( + hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[i].hz, + hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[i].uhz); + + tmp = dur_ms / (1000000 / (sensor_odr / 1000)); + val = (u8)tmp; + max_dur = ST_LSM6DSRX_WAKE_UP_DUR_MASK >> + __ffs(ST_LSM6DSRX_WAKE_UP_DUR_MASK); + if (val > max_dur) + val = max_dur; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_WAKE_UP_DUR_ADDR, + ST_LSM6DSRX_WAKE_UP_DUR_MASK, + val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[1] = dur_ms; + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[i].hz; + + return 0; +} + +/* + * st_lsm6dsrx_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_lsm6dsrx_set_freefall_threshold(struct st_lsm6dsrx_hw *hw, + int th_mg) +{ + struct st_lsm6dsrx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_free_fall_threshold); i++) { + if (th_mg >= st_lsm6dsrx_free_fall_threshold[i].mg) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsrx_free_fall_threshold)) + return -EINVAL; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_FREE_FALL_ADDR, + ST_LSM6DSRX_FF_THS_MASK, + st_lsm6dsrx_free_fall_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_FF]; + sensor = iio_priv(iio_dev); + sensor->conf[2] = th_mg; + + return 0; +} + +/* + * st_lsm6dsrx_set_6D_threshold - set 6D threshold detection in degrees + * + * @hw - ST IMU MEMS hw instance + * @deg - 6D threshold in degrees + */ +static int st_lsm6dsrx_set_6D_threshold(struct st_lsm6dsrx_hw *hw, + int deg) +{ + struct st_lsm6dsrx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_6D_threshold); i++) { + if (deg >= st_lsm6dsrx_6D_threshold[i].deg) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsrx_6D_threshold)) + return -EINVAL; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_TAP_THS_6D_ADDR, + ST_LSM6DSRX_SIXD_THS_MASK, + st_lsm6dsrx_6D_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_6D]; + sensor = iio_priv(iio_dev); + sensor->conf[3] = deg; + + return 0; +} + +/* + * st_lsm6dsrx_init_tap - initialize tap detection to default value + * + * @hw - ST IMU MEMS hw instance + */ +static int st_lsm6dsrx_init_tap(struct st_lsm6dsrx_hw *hw) +{ + int err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_TAP_CFG0_ADDR, + ST_LSM6DSRX_REG_TAP_EN_MASK, + FIELD_PREP(ST_LSM6DSRX_REG_TAP_EN_MASK, + 0x07)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_TAP_CFG1_ADDR, + ST_LSM6DSRX_TAP_THS_X_MASK, + FIELD_PREP(ST_LSM6DSRX_TAP_THS_X_MASK, + 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_TAP_CFG2_ADDR, + ST_LSM6DSRX_TAP_THS_Y_MASK, + FIELD_PREP(ST_LSM6DSRX_TAP_THS_Y_MASK, + 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_TAP_THS_6D_ADDR, + ST_LSM6DSRX_TAP_THS_Z_MASK, + FIELD_PREP(ST_LSM6DSRX_TAP_THS_Z_MASK, + 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_INT_DUR2_ADDR, + ST_LSM6DSRX_SHOCK_MASK, + FIELD_PREP(ST_LSM6DSRX_SHOCK_MASK, + 0x02)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSRX_REG_INT_DUR2_ADDR, + ST_LSM6DSRX_QUIET_MASK, + FIELD_PREP(ST_LSM6DSRX_QUIET_MASK, + 0x01)); + + return err < 0 ? err : 0; +} + +static int +st_lsm6dsrx_event_sensor_set_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + int err, eint = !!enable; + struct st_lsm6dsrx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSRX_REG_MD1_CFG_ADDR : + ST_LSM6DSRX_REG_MD2_CFG_ADDR; + + err = st_lsm6dsrx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + switch (sensor->id) { + case ST_LSM6DSRX_ID_WK: + err = st_lsm6dsrx_write_with_mask_locked(hw, int_reg, + ST_LSM6DSRX_INT_WU_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSRX_ID_FF: + err = st_lsm6dsrx_write_with_mask_locked(hw, int_reg, + ST_LSM6DSRX_INT_FF_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSRX_ID_SLPCHG: + err = st_lsm6dsrx_write_with_mask_locked(hw, int_reg, + ST_LSM6DSRX_INT_SLEEP_CHANGE_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSRX_ID_6D: + err = st_lsm6dsrx_write_with_mask_locked(hw, int_reg, + ST_LSM6DSRX_INT_6D_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSRX_ID_TAP: + err = st_lsm6dsrx_write_with_mask_locked(hw, int_reg, + ST_LSM6DSRX_INT_SINGLE_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSRX_SINGLE_DOUBLE_TAP_MASK, + 0); + if (err < 0) + return err; + break; + case ST_LSM6DSRX_ID_DTAP: + err = st_lsm6dsrx_write_with_mask_locked(hw, int_reg, + ST_LSM6DSRX_INT_DOUBLE_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSRX_SINGLE_DOUBLE_TAP_MASK, + 1); + if (err < 0) + return err; + break; + default: + err = -EINVAL; + break; + } + + if (err >= 0) { + err = st_lsm6dsrx_write_with_mask_locked(hw, + ST_LSM6DSRX_REG_TAP_CFG2_ADDR, + ST_LSM6DSRX_INTERRUPTS_ENABLE_MASK, + eint); + if (eint == 0) + hw->enable_mask &= ~BIT_ULL(sensor->id); + else + hw->enable_mask |= BIT_ULL(sensor->id); + } + + return err; +} + +static int +st_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT_ULL(sensor->id)); +} + +static int +st_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_lsm6dsrx_event_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_lsm6dsrx_wakeup_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[0]); +} + +static ssize_t +st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_wakeup_duration_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[1]); +} + +static ssize_t +st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_freefall_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[2]); +} + +static ssize_t +st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_6D_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[3]); +} + +static ssize_t +st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_wakeup_threshold_get, + st_lsm6dsrx_wakeup_threshold_set, 0); + +static IIO_DEVICE_ATTR(wakeup_duration, 0644, + st_lsm6dsrx_wakeup_duration_get, + st_lsm6dsrx_wakeup_duration_set, 0); + +static IIO_DEVICE_ATTR(freefall_threshold, 0644, + st_lsm6dsrx_freefall_threshold_get, + st_lsm6dsrx_freefall_threshold_set, 0); + +static IIO_DEVICE_ATTR(sixd_threshold, 0644, + st_lsm6dsrx_6D_threshold_get, + st_lsm6dsrx_6D_threshold_set, 0); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsrx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_wk_attribute_group = { + .attrs = st_lsm6dsrx_wk_attributes, +}; + +static const struct iio_info st_lsm6dsrx_wk_info = { + .attrs = &st_lsm6dsrx_wk_attribute_group, +}; + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_ff_attribute_group = { + .attrs = st_lsm6dsrx_ff_attributes, +}; + +static const struct iio_info st_lsm6dsrx_ff_info = { + .attrs = &st_lsm6dsrx_ff_attribute_group, + .read_event_config = st_lsm6dsrx_read_event_config, + .write_event_config = st_lsm6dsrx_write_event_config, +}; + +static struct attribute *st_lsm6dsrx_sc_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsrx_sc_attribute_group = { + .attrs = st_lsm6dsrx_sc_attributes, +}; + +static const struct iio_info st_lsm6dsrx_sc_info = { + .attrs = &st_lsm6dsrx_sc_attribute_group, + .read_event_config = st_lsm6dsrx_read_event_config, + .write_event_config = st_lsm6dsrx_write_event_config, +}; + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_6D_attribute_group = { + .attrs = st_lsm6dsrx_6D_attributes, +}; + +static const struct iio_info st_lsm6dsrx_6D_info = { + .attrs = &st_lsm6dsrx_6D_attribute_group, +}; + +static struct attribute *st_lsm6dsrx_tap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsrx_tap_attribute_group = { + .attrs = st_lsm6dsrx_tap_attributes, +}; + +static const struct iio_info st_lsm6dsrx_tap_info = { + .attrs = &st_lsm6dsrx_tap_attribute_group, + .read_event_config = st_lsm6dsrx_read_event_config, + .write_event_config = st_lsm6dsrx_write_event_config, +}; + +static struct attribute *st_lsm6dsrx_dtap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsrx_dtap_attribute_group = { + .attrs = st_lsm6dsrx_dtap_attributes, +}; + +static const struct iio_info st_lsm6dsrx_dtap_info = { + .attrs = &st_lsm6dsrx_dtap_attribute_group, + .read_event_config = st_lsm6dsrx_read_event_config, + .write_event_config = st_lsm6dsrx_write_event_config, +}; + +static struct iio_dev * +st_lsm6dsrx_alloc_event_iiodev(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_sensor_id id) +{ + struct st_lsm6dsrx_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_lsm6dsrx_event_available_scan_masks; + + switch (id) { + case ST_LSM6DSRX_ID_WK: + iio_dev->channels = st_lsm6dsrx_wk_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_wk_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_wk", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_wk_info; + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[2].hz; + break; + case ST_LSM6DSRX_ID_FF: + iio_dev->channels = st_lsm6dsrx_ff_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_ff_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ff", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_ff_info; + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[2].hz; + break; + case ST_LSM6DSRX_ID_SLPCHG: + iio_dev->channels = st_lsm6dsrx_sc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_sc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sc", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_sc_info; + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[2].hz; + break; + case ST_LSM6DSRX_ID_6D: + iio_dev->channels = st_lsm6dsrx_6D_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_6D_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_6d", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_6D_info; + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[2].hz; + break; + case ST_LSM6DSRX_ID_TAP: + iio_dev->channels = st_lsm6dsrx_tap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_tap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tap", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_tap_info; + + /* require main sensor odr to 416 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[6].hz; + break; + case ST_LSM6DSRX_ID_DTAP: + iio_dev->channels = st_lsm6dsrx_dtap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_dtap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_dtap", hw->settings->id.name); + iio_dev->info = &st_lsm6dsrx_dtap_info; + + /* require main sensor odr to 416 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSRX_ID_ACC].odr_avl[6].hz; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/** + * st_lsm6dsrx_event_handler() - Detect embedded low power event + * + * @hw: ST IMU MEMS hw instance. + * + * return IRQ_HANDLED. + * + * NOTE: Uses page_lock through the st_lsm6dsrx_read_locked. + */ +int st_lsm6dsrx_event_handler(struct st_lsm6dsrx_hw *hw) +{ + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + if (hw->enable_mask & + (BIT_ULL(ST_LSM6DSRX_ID_WK) | BIT_ULL(ST_LSM6DSRX_ID_FF) | + BIT_ULL(ST_LSM6DSRX_ID_SLPCHG) | + BIT_ULL(ST_LSM6DSRX_ID_6D) | BIT_ULL(ST_LSM6DSRX_ID_TAP) | + BIT_ULL(ST_LSM6DSRX_ID_DTAP))) { + err = st_lsm6dsrx_read_locked(hw, + ST_LSM6DSRX_REG_ALL_INT_SRC_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* base function sensors */ + if (status & ST_LSM6DSRX_SINGLE_TAP_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_TAP]; + event = IIO_UNMOD_EVENT_CODE(IIO_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_LSM6DSRX_DOUBLE_TAP_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_DTAP]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSRX_FF_IA_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_FF]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSRX_WU_IA_MASK) { + struct st_lsm6dsrx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_WK]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + + if (status & ST_LSM6DSRX_SLEEP_CHANGE_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_SLPCHG]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSRX_D6D_IA_MASK) { + struct st_lsm6dsrx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LSM6DSRX_ID_6D]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + } + + return IRQ_HANDLED; +} + +static inline int st_lsm6dsrx_get_6D(struct st_lsm6dsrx_hw *hw, u8 *out) +{ + return st_lsm6dsrx_read_with_mask(hw, ST_LSM6DSRX_REG_D6D_SRC_ADDR, + ST_LSM6DSRX_D6D_EVENT_MASK, out); +} + +static inline int st_lsm6dsrx_get_wk(struct st_lsm6dsrx_hw *hw, u8 *out) +{ + return st_lsm6dsrx_read_with_mask(hw, + ST_LSM6DSRX_REG_WAKE_UP_SRC_ADDR, + ST_LSM6DSRX_WAKE_UP_EVENT_MASK, out); +} + +static irqreturn_t st_lsm6dsrx_6D_handler_thread(int irq, void *p) +{ + u8 iio_buf[ALIGN(1, sizeof(s64)) + sizeof(s64)]; + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + + st_lsm6dsrx_get_6D(sensor->hw, iio_buf); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, + iio_get_time_ns(iio_dev)); + + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +static irqreturn_t st_lsm6dsrx_wk_handler_thread(int irq, void *p) +{ + u8 iio_buf[ALIGN(1, sizeof(s64)) + sizeof(s64)]; + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + + st_lsm6dsrx_get_wk(sensor->hw, iio_buf); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, + iio_get_time_ns(iio_dev)); + + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +int st_lsm6dsrx_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig); + struct st_lsm6dsrx_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_lsm6dsrx_trigger_ops = { + .set_trigger_state = &st_lsm6dsrx_trig_set_state, +}; + +static int st_lsm6dsrx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsrx_event_sensor_set_enable(iio_priv(iio_dev), + true); +} + +static int st_lsm6dsrx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsrx_event_sensor_set_enable(iio_priv(iio_dev), + false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsrx_buffer_ops = { + .preenable = st_lsm6dsrx_buffer_preenable, +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0) + .postenable = iio_triggered_buffer_postenable, + .predisable = iio_triggered_buffer_predisable, +#endif /* LINUX_VERSION_CODE */ + .postdisable = st_lsm6dsrx_buffer_postdisable, +}; + +static int +st_lsm6dsrx_config_default_events(struct st_lsm6dsrx_hw *hw) +{ + int err; + + /* set default wake-up thershold to 93750 ug */ + err = st_lsm6dsrx_set_wake_up_thershold(hw, 93750); + if (err < 0) + return err; + + /* set default wake-up duration to 0 */ + err = st_lsm6dsrx_set_wake_up_duration(hw, 0); + if (err < 0) + return err; + + /* set default FF threshold to 312 mg */ + err = st_lsm6dsrx_set_freefall_threshold(hw, 312); + if (err < 0) + return err; + + /* set default 6D threshold to 60 degrees */ + err = st_lsm6dsrx_set_6D_threshold(hw, 60); + if (err < 0) + return err; + + return st_lsm6dsrx_init_tap(hw); +} + +int st_lsm6dsrx_probe_event(struct st_lsm6dsrx_hw *hw) +{ + struct st_lsm6dsrx_sensor *sensor; + struct iio_dev *iio_dev; + irqreturn_t (*pthread[ST_LSM6DSRX_ID_MAX - ST_LSM6DSRX_ID_WK])(int irq, void *p) = { + [0] = st_lsm6dsrx_wk_handler_thread, + [1] = st_lsm6dsrx_6D_handler_thread, + /* add here all other trigger handler funcions */ + }; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_event_sensor_list); + i++) { + enum st_lsm6dsrx_sensor_id id = + st_lsm6dsrx_event_sensor_list[i]; + + hw->iio_devs[id] = st_lsm6dsrx_alloc_event_iiodev(hw, + id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + /* configure trigger sensors */ + for (i = 0; + i < ARRAY_SIZE(st_lsm6dsrx_event_trigger_sensor_list); + i++) { + enum st_lsm6dsrx_sensor_id id = + st_lsm6dsrx_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[i - ST_LSM6DSRX_ID_WK], + &st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_config_default_events(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_hwtimestamp.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_hwtimestamp.c new file mode 100644 index 000000000000..77b510b50bca --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_hwtimestamp.c @@ -0,0 +1,116 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx hwtimestamp library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +#define ST_LSM6DSRX_TSYNC_OFFSET_NS (300 * 1000LL) + +static void st_lsm6dsrx_read_hw_timestamp(struct st_lsm6dsrx_hw *hw) +{ + s64 timestamp_hw_global; + s64 eventLSB, eventMSB; + __le32 timestamp_hw; + s64 timestamp_cpu; + __le32 tmp; + int err; + + err = st_lsm6dsrx_read_locked(hw, ST_LSM6DSRX_REG_TIMESTAMP0_ADDR, + (u8 *)×tamp_hw, + sizeof(timestamp_hw)); + if (err < 0) + return; + + timestamp_cpu = iio_get_time_ns(hw->iio_devs[0]) - + ST_LSM6DSRX_TSYNC_OFFSET_NS; + + eventLSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 0, + IIO_EV_TYPE_TIME_SYNC, 0, 0, 0); + eventMSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 1, + 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_LSM6DSRX_ID_GYRO)) { + iio_push_event(hw->iio_devs[ST_LSM6DSRX_ID_GYRO], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSRX_ID_GYRO], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSRX_ID_ACC)) { + iio_push_event(hw->iio_devs[ST_LSM6DSRX_ID_ACC], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSRX_ID_ACC], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSRX_ID_TEMP)) { + iio_push_event(hw->iio_devs[ST_LSM6DSRX_ID_TEMP], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSRX_ID_TEMP], eventMSB, + timestamp_cpu); + } + + if (hw->timesync_c < 6) + hw->timesync_c++; + else + hw->timesync_ktime = ktime_set(0, ST_LSM6DSRX_DEFAULT_KTIME); +} + +static void st_lsm6dsrx_timesync_fn(struct work_struct *work) +{ + struct st_lsm6dsrx_hw *hw = container_of(work, struct st_lsm6dsrx_hw, + timesync_work); + + st_lsm6dsrx_read_hw_timestamp(hw); +} + +static enum hrtimer_restart st_lsm6dsrx_timer_fn(struct hrtimer *timer) +{ + struct st_lsm6dsrx_hw *hw; + + hw = container_of(timer, struct st_lsm6dsrx_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_lsm6dsrx_hwtimesync_init(struct st_lsm6dsrx_hw *hw) +{ + hw->timesync_c = 0; + hw->timesync_ktime = ktime_set(0, ST_LSM6DSRX_DEFAULT_KTIME); + hrtimer_init(&hw->timesync_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + hw->timesync_timer.function = st_lsm6dsrx_timer_fn; + + spin_lock_init(&hw->hwtimestamp_lock); + hw->hw_timestamp_global = 0; + + hw->timesync_workqueue = create_singlethread_workqueue("st_lsm6dsrx_workqueue"); + if (!hw->timesync_workqueue) + return -ENOMEM; + + INIT_WORK(&hw->timesync_work, st_lsm6dsrx_timesync_fn); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsrx_hwtimesync_init); diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i2c.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i2c.c new file mode 100644 index 000000000000..baac1a09216d --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i2c.c @@ -0,0 +1,79 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +static const struct regmap_config st_lsm6dsrx_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_probe(&client->dev, client->irq, + hw_id, regmap); +} + +static int st_lsm6dsrx_i2c_remove(struct i2c_client *client) +{ + return st_lsm6dsrx_mlc_remove(&client->dev); +} + +static const struct of_device_id st_lsm6dsrx_i2c_of_match[] = { + { + .compatible = "st," ST_LSM6DSRX_DEV_NAME, + .data = (void *)ST_LSM6DSRX_ID, + }, + { + .compatible = "st," ST_LSM6DSR_DEV_NAME, + .data = (void *)ST_LSM6DSR_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsrx_i2c_of_match); + +static const struct i2c_device_id st_lsm6dsrx_i2c_id_table[] = { + { ST_LSM6DSRX_DEV_NAME, ST_LSM6DSRX_ID }, + { ST_LSM6DSR_DEV_NAME, ST_LSM6DSR_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsrx_i2c_id_table); + +static struct i2c_driver st_lsm6dsrx_driver = { + .driver = { + .name = "st_" ST_LSM6DSRX_DEV_NAME "_i2c", + .pm = &st_lsm6dsrx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsrx_i2c_of_match), + }, + .probe = st_lsm6dsrx_i2c_probe, + .remove = st_lsm6dsrx_i2c_remove, + .id_table = st_lsm6dsrx_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsrx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsrx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i3c.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i3c.c new file mode 100644 index 000000000000..2ec481b30781 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_i3c.c @@ -0,0 +1,58 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx i3c driver + * + * MEMS Software Solutions Team + * + * Copyright 2020 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +static const struct i3c_device_id st_lsm6dsrx_i3c_ids[] = { + I3C_DEVICE(0x0104, ST_LSM6DSRX_WHOAMI_VAL, (void *)ST_LSM6DSRX_ID), + {}, +}; +MODULE_DEVICE_TABLE(i3c, st_lsm6dsrx_i3c_ids); + +static int st_lsm6dsrx_i3c_probe(struct i3c_device *i3cdev) +{ + struct regmap_config st_lsm6dsrx_i3c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + }; + const struct i3c_device_id *id = + i3c_device_match_id(i3cdev, st_lsm6dsrx_i3c_ids); + struct regmap *regmap; + + regmap = devm_regmap_init_i3c(i3cdev, &st_lsm6dsrx_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_lsm6dsrx_probe(&i3cdev->dev, 0, (uintptr_t)id->data, regmap); +} + +static struct i3c_driver st_lsm6dsrx_driver = { + .driver = { + .name = "st_" ST_LSM6DSRX_DEV_NAME "_i3c", + .pm = &st_lsm6dsrx_pm_ops, + }, + .probe = st_lsm6dsrx_i3c_probe, + .id_table = st_lsm6dsrx_i3c_ids, +}; +module_i3c_driver(st_lsm6dsrx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsrx i3c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_mlc.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_mlc.c new file mode 100644 index 000000000000..4bd8864c4bc8 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_mlc.c @@ -0,0 +1,861 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx machine learning core driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsrx.h" + +#define ST_LSM6DSRX_MLC_LOADER_VERSION "0.3" + +/* number of machine learning core available on device hardware */ +#define ST_LSM6DSRX_MLC_MAX_NUMBER 8 +#define ST_LSM6DSRX_FSM_MAX_NUMBER 16 + +#ifdef CONFIG_IIO_LSM6DSRX_MLC_BUILTIN_FIRMWARE +static const u8 st_lsm6dsrx_mlc_fw[] = { + #include "st_lsm6dsrx_mlc.fw" +}; +DECLARE_BUILTIN_FIRMWARE(LSM6DSRX_MLC_FIRMWARE_NAME, st_lsm6dsrx_mlc_fw); +#else /* CONFIG_IIO_LSM6DSRX_MLC_BUILTIN_FIRMWARE */ +#define LSM6DSRX_MLC_FIRMWARE_NAME "st_lsm6dsrx_mlc.bin" +#endif /* CONFIG_IIO_LSM6DSRX_MLC_BUILTIN_FIRMWARE */ + +#ifdef CONFIG_IIO_ST_LSM6DSRX_MLC_PRELOAD +#include "st_lsm6dsrx_preload_mlc.h" +#endif /* CONFIG_IIO_ST_LSM6DSRX_MLC_PRELOAD */ + +/* converts MLC odr to main sensor trigger odr (acc) */ +static const uint16_t mlc_odr_data[] = { + [0x00] = 0, + [0x01] = 12, + [0x02] = 26, + [0x03] = 52, + [0x04] = 104, + [0x05] = 208, + [0x06] = 416, + [0x07] = 833, +}; + +static +struct iio_dev *st_lsm6dsrx_mlc_alloc_iio_dev(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_sensor_id id); + +static const unsigned long st_lsm6dsrx_mlc_available_scan_masks[] = { + 0x1, 0x0 +}; + +static inline int +st_lsm6dsrx_read_page_locked(struct st_lsm6dsrx_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + st_lsm6dsrx_set_page_access(hw, true, ST_LSM6DSRX_REG_FUNC_CFG_MASK); + err = regmap_bulk_read(hw->regmap, addr, val, len); + st_lsm6dsrx_set_page_access(hw, false, ST_LSM6DSRX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsrx_write_page_locked(struct st_lsm6dsrx_hw *hw, unsigned int addr, + unsigned int *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsrx_set_page_access(hw, true, ST_LSM6DSRX_REG_FUNC_CFG_MASK); + err = regmap_bulk_write(hw->regmap, addr, val, len); + st_lsm6dsrx_set_page_access(hw, false, ST_LSM6DSRX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsrx_update_page_bits_locked(struct st_lsm6dsrx_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsrx_set_page_access(hw, true, ST_LSM6DSRX_REG_FUNC_CFG_MASK); + err = regmap_update_bits(hw->regmap, addr, mask, val); + st_lsm6dsrx_set_page_access(hw, false, ST_LSM6DSRX_REG_FUNC_CFG_MASK); + mutex_unlock(&hw->page_lock); + + return err; +} + +static int st_lsm6dsrx_mlc_enable_sensor(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsrx_hw *hw = sensor->hw; + int i, id, err; + + /* enable acc sensor as trigger */ + err = st_lsm6dsrx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + if (sensor->status == ST_LSM6DSRX_MLC_ENABLED) { + int value; + + value = enable ? hw->mlc_config->mlc_int_mask : 0; + err = st_lsm6dsrx_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_lsm6dsrx_update_page_bits_locked(hw, + ST_LSM6DSRX_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSRX_MLC_EN_MASK, + ST_LSM6DSRX_SHIFT_VAL(enable, + ST_LSM6DSRX_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_LSM6DSRX_FSM_ENABLED) { + int value[2]; + + value[0] = enable ? hw->mlc_config->fsm_int_mask[0] : 0; + value[1] = enable ? hw->mlc_config->fsm_int_mask[1] : 0; + err = st_lsm6dsrx_write_page_locked(hw, + hw->mlc_config->fsm_int_addr[0], + &value[0], 2); + if (err < 0) + return err; + + /* enable fsm core */ + for (i = 0; i < ST_LSM6DSRX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsrx_fsm_sensor_list[i]; + if (hw->enable_mask & BIT_ULL(id)) + break; + } + + /* check for any other fsm already enabled */ + if (enable || i == ST_LSM6DSRX_FSM_MAX_NUMBER) { + err = st_lsm6dsrx_update_page_bits_locked(hw, + ST_LSM6DSRX_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSRX_FSM_EN_MASK, + ST_LSM6DSRX_SHIFT_VAL(enable, + ST_LSM6DSRX_FSM_EN_MASK)); + if (err < 0) + return err; + } + + dev_info(sensor->hw->dev, + "Enabling FSM sensor %d to %d (INT %x-%x)\n", + sensor->id, enable, value[0], value[1]); + } else { + dev_err(hw->dev, "invalid sensor configuration\n"); + err = -ENODEV; + + return err; + } + + if (enable) + hw->enable_mask |= BIT_ULL(sensor->id); + else + hw->enable_mask &= ~BIT_ULL(sensor->id); + + return 0; +} + +static int st_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsrx_mlc_enable_sensor(sensor, state); +} + +static int st_lsm6dsrx_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_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsrx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT_ULL(sensor->id)); +} + +/* + * st_lsm6dsrx_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_lsm6dsrx_verify_mlc_fsm_support(const struct firmware *fw, + struct st_lsm6dsrx_hw *hw) +{ + bool stmc_page = false; + uint8_t reg, val; + int i = 0; + + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == 0x01 && val == 0x80) { + stmc_page = true; + } else if (reg == 0x01 && val == 0x00) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSRX_MLC_INT1_ADDR: + case ST_LSM6DSRX_MLC_INT2_ADDR: + if (!hw->settings->st_mlc_probe) + return -ENODEV; + break; + case ST_LSM6DSRX_FSM_INT1_A_ADDR: + case ST_LSM6DSRX_FSM_INT2_A_ADDR: + case ST_LSM6DSRX_FSM_INT1_B_ADDR: + case ST_LSM6DSRX_FSM_INT2_B_ADDR: + if (!hw->settings->st_fsm_probe) + return -ENODEV; + break; + default: + break; + } + } + } + + return 0; +} + +/* parse fw and program MLC/FSM fragments */ +static int st_lsm6dsrx_program_mlc(const struct firmware *fw, + struct st_lsm6dsrx_hw *hw) +{ + uint8_t mlc_int = 0, mlc_num = 0, fsm_num = 0, skip = 0; + uint8_t fsm_int[2] = { 0, 0 }; + uint8_t reg, val, req_odr = 0; + int int_pin, ret, i = 0; + bool stmc_page = false; + + mutex_lock(&hw->page_lock); + while (i < fw->size) { + reg = fw->data[i++]; + val = fw->data[i++]; + + if (reg == 0x01 && val == 0x80) { + stmc_page = true; + } else if (reg == 0x01 && val == 0x00) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSRX_MLC_INT1_ADDR: + case ST_LSM6DSRX_MLC_INT2_ADDR: + mlc_int |= val; + mlc_num++; + skip = 1; + break; + case ST_LSM6DSRX_FSM_INT1_A_ADDR: + case ST_LSM6DSRX_FSM_INT2_A_ADDR: + fsm_int[0] |= val; + fsm_num++; + skip = 1; + break; + case ST_LSM6DSRX_FSM_INT1_B_ADDR: + case ST_LSM6DSRX_FSM_INT2_B_ADDR: + fsm_int[1] |= val; + fsm_num++; + skip = 1; + break; + case ST_LSM6DSRX_EMB_FUNC_EN_B_ADDR: + skip = 1; + break; + default: + break; + } + } else if (reg == 0x10) { + /* save requested odr and skip write to reg */ + req_odr = max_t(uint8_t, req_odr, ((val >> 4) & 0x07)); + skip = 1; + } + + if (!skip) { + ret = regmap_write(hw->regmap, reg, val); + if (ret) { + mutex_unlock(&hw->page_lock); + dev_err(hw->dev, "regmap_write fails\n"); + + return ret; + } + } + + skip = 0; + + if (mlc_num >= ST_LSM6DSRX_MLC_MAX_NUMBER || + fsm_num >= ST_LSM6DSRX_FSM_MAX_NUMBER) + break; + } + + hw->mlc_config->bin_len = fw->size; + + ret = st_lsm6dsrx_of_get_pin(hw, &int_pin); + if (ret < 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; + } + + if (mlc_num) { + hw->mlc_config->mlc_int_mask = mlc_int; + + hw->mlc_config->mlc_int_addr = (int_pin == 1 ? + ST_LSM6DSRX_MLC_INT1_ADDR : + ST_LSM6DSRX_MLC_INT2_ADDR); + + hw->mlc_config->status |= ST_LSM6DSRX_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[0] = fsm_int[0]; + hw->mlc_config->fsm_int_mask[1] = fsm_int[1]; + + hw->mlc_config->fsm_int_addr[0] = (int_pin == 1 ? + ST_LSM6DSRX_FSM_INT1_A_ADDR : + ST_LSM6DSRX_FSM_INT2_A_ADDR); + hw->mlc_config->fsm_int_addr[1] = (int_pin == 1 ? + ST_LSM6DSRX_FSM_INT1_B_ADDR : + ST_LSM6DSRX_FSM_INT2_B_ADDR); + + hw->mlc_config->status |= ST_LSM6DSRX_FSM_ENABLED; + hw->mlc_config->fsm_configured += fsm_num; + hw->mlc_config->requested_odr = mlc_odr_data[req_odr]; + } + + mutex_unlock(&hw->page_lock); + + return fsm_num + mlc_num; +} + +static void st_lsm6dsrx_mlc_update(const struct firmware *fw, void *context) +{ + struct st_lsm6dsrx_hw *hw = context; + enum st_lsm6dsrx_sensor_id id; + int ret, i; + + if (!fw) { + dev_err(hw->dev, "could not get binary firmware\n"); + return; + } + + ret = st_lsm6dsrx_verify_mlc_fsm_support(fw, hw); + if (ret) { + dev_err(hw->dev, "invalid file format for device\n"); + return; + } + + ret = st_lsm6dsrx_program_mlc(fw, hw); + if (ret > 0) { + u16 fsm_mask = *(u16 *)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_LSM6DSRX_MLC_MAX_NUMBER; i++) { + if (mlc_mask & BIT(i)) { + id = st_lsm6dsrx_mlc_sensor_list[i]; + hw->iio_devs[id] = + st_lsm6dsrx_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_LSM6DSRX_FSM_MAX_NUMBER; i++) { + if (fsm_mask & BIT(i)) { + id = st_lsm6dsrx_fsm_sensor_list[i]; + hw->iio_devs[id] = + st_lsm6dsrx_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_lsm6dsrx_mlc_flush_single(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_sensor_id id) +{ + struct st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_mlc_flush_all(struct st_lsm6dsrx_hw *hw) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_mlc_sensor_list); i++) + st_lsm6dsrx_mlc_flush_single(hw, + st_lsm6dsrx_mlc_sensor_list[i]); + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsrx_fsm_sensor_list); i++) + st_lsm6dsrx_mlc_flush_single(hw, + st_lsm6dsrx_fsm_sensor_list[i]); + + return 0; +} + +static ssize_t st_lsm6dsrx_mlc_info(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsrx_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_lsm6dsrx_mlc_get_version(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "loader version %s\n", + ST_LSM6DSRX_MLC_LOADER_VERSION); +} + +static ssize_t st_lsm6dsrx_mlc_flush(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsrx_hw *hw = sensor->hw; + int ret; + + ret = st_lsm6dsrx_mlc_flush_all(hw); + memset(hw->mlc_config, 0, sizeof(*hw->mlc_config)); + + return ret < 0 ? ret : size; +} + +static ssize_t st_lsm6dsrx_mlc_upload_firmware(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err; + + err = request_firmware_nowait(THIS_MODULE, true, + LSM6DSRX_MLC_FIRMWARE_NAME, + dev, GFP_KERNEL, + sensor->hw, + st_lsm6dsrx_mlc_update); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6dsrx_mlc_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsrx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%d\n", hw->mlc_config->requested_odr); +} + +static IIO_DEVICE_ATTR(config_info, 0444, + st_lsm6dsrx_mlc_info, NULL, 0); +static IIO_DEVICE_ATTR(flush_config, 0200, + NULL, st_lsm6dsrx_mlc_flush, 0); +static IIO_DEVICE_ATTR(loader_version, 0444, + st_lsm6dsrx_mlc_get_version, NULL, 0); +static IIO_DEVICE_ATTR(load_mlc, 0200, + NULL, st_lsm6dsrx_mlc_upload_firmware, 0); +static IIO_DEVICE_ATTR(odr, 0444, + st_lsm6dsrx_mlc_odr, NULL, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsrx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_mlc_event_attribute_group = { + .attrs = st_lsm6dsrx_mlc_event_attributes, +}; + +static const struct iio_info st_lsm6dsrx_mlc_event_info = { + .attrs = &st_lsm6dsrx_mlc_event_attribute_group, + .read_event_config = st_lsm6dsrx_mlc_read_event_config, + .write_event_config = st_lsm6dsrx_mlc_write_event_config, +}; + +static ssize_t st_lsm6dsrx_mlc_x_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_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_lsm6dsrx_mlc_x_odr, NULL, 0); + +static struct attribute *st_lsm6dsrx_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_lsm6dsrx_mlc_x_event_attribute_group = { + .attrs = st_lsm6dsrx_mlc_x_event_attributes, +}; +static const struct iio_info st_lsm6dsrx_mlc_x_event_info = { + .attrs = &st_lsm6dsrx_mlc_x_event_attribute_group, + .read_event_config = st_lsm6dsrx_mlc_read_event_config, + .write_event_config = st_lsm6dsrx_mlc_write_event_config, +}; + +static +struct iio_dev *st_lsm6dsrx_mlc_alloc_iio_dev(struct st_lsm6dsrx_hw *hw, + enum st_lsm6dsrx_sensor_id id) +{ + struct st_lsm6dsrx_sensor *sensor; + struct iio_chan_spec *channels; + struct iio_dev *iio_dev; + + /* devm management only for ST_LSM6DSRX_ID_MLC */ + if (id == ST_LSM6DSRX_ID_MLC) { + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + } else { +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,9,0) + 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; + sensor->pm = ST_LSM6DSRX_NO_MODE; + + switch (id) { + case ST_LSM6DSRX_ID_MLC: { + const struct iio_chan_spec st_lsm6dsrx_mlc_channels[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = devm_kzalloc(hw->dev, + sizeof(st_lsm6dsrx_mlc_channels), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsrx_mlc_channels, + sizeof(st_lsm6dsrx_mlc_channels)); + + iio_dev->available_scan_masks = + st_lsm6dsrx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_mlc_channels); + iio_dev->info = &st_lsm6dsrx_mlc_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_loader", hw->settings->id.name); + break; + } + case ST_LSM6DSRX_ID_MLC_0: + case ST_LSM6DSRX_ID_MLC_1: + case ST_LSM6DSRX_ID_MLC_2: + case ST_LSM6DSRX_ID_MLC_3: + case ST_LSM6DSRX_ID_MLC_4: + case ST_LSM6DSRX_ID_MLC_5: + case ST_LSM6DSRX_ID_MLC_6: + case ST_LSM6DSRX_ID_MLC_7: { + const struct iio_chan_spec st_lsm6dsrx_mlc_x_ch[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsrx_mlc_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsrx_mlc_x_ch, + sizeof(st_lsm6dsrx_mlc_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsrx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_mlc_x_ch); + iio_dev->info = &st_lsm6dsrx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc_%d", hw->settings->id.name, + id - ST_LSM6DSRX_ID_MLC_0); + sensor->outreg_addr = ST_LSM6DSRX_REG_MLC0_SRC_ADDR + + id - ST_LSM6DSRX_ID_MLC_0; + sensor->status = ST_LSM6DSRX_MLC_ENABLED; + sensor->odr = hw->mlc_config->requested_odr; + sensor->uodr = 0; + + break; + } + case ST_LSM6DSRX_ID_FSM_0: + case ST_LSM6DSRX_ID_FSM_1: + case ST_LSM6DSRX_ID_FSM_2: + case ST_LSM6DSRX_ID_FSM_3: + case ST_LSM6DSRX_ID_FSM_4: + case ST_LSM6DSRX_ID_FSM_5: + case ST_LSM6DSRX_ID_FSM_6: + case ST_LSM6DSRX_ID_FSM_7: + case ST_LSM6DSRX_ID_FSM_8: + case ST_LSM6DSRX_ID_FSM_9: + case ST_LSM6DSRX_ID_FSM_10: + case ST_LSM6DSRX_ID_FSM_11: + case ST_LSM6DSRX_ID_FSM_12: + case ST_LSM6DSRX_ID_FSM_13: + case ST_LSM6DSRX_ID_FSM_14: + case ST_LSM6DSRX_ID_FSM_15: { + const struct iio_chan_spec st_lsm6dsrx_fsm_x_ch[] = { + ST_LSM6DSRX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsrx_fsm_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsrx_fsm_x_ch, + sizeof(st_lsm6dsrx_fsm_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsrx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsrx_fsm_x_ch); + iio_dev->info = &st_lsm6dsrx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_fsm_%d", hw->settings->id.name, + id - ST_LSM6DSRX_ID_FSM_0); + sensor->outreg_addr = ST_LSM6DSRX_FSM_OUTS1_ADDR + + id - ST_LSM6DSRX_ID_FSM_0; + sensor->status = ST_LSM6DSRX_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_lsm6dsrx_mlc_check_status(struct st_lsm6dsrx_hw *hw) +{ + struct st_lsm6dsrx_sensor *sensor; + u8 i, mlc_status, id, event[16]; + struct iio_dev *iio_dev; + __le16 __fsm_status = 0; + u16 fsm_status; + int err = 0; + + if (hw->mlc_config->status & ST_LSM6DSRX_MLC_ENABLED) { + err = st_lsm6dsrx_read_locked(hw, + ST_LSM6DSRX_MLC_STATUS_MAINPAGE, + (void *)&mlc_status, 1); + if (err) + return err; + + if (mlc_status) { + for (i = 0; i < ST_LSM6DSRX_MLC_MAX_NUMBER; i++) { + id = st_lsm6dsrx_mlc_sensor_list[i]; + if (!(hw->enable_mask & BIT_ULL(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_lsm6dsrx_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_LSM6DSRX_FSM_ENABLED) { + err = st_lsm6dsrx_read_locked(hw, + ST_LSM6DSRX_FSM_STATUS_A_MAINPAGE, + (void *)&__fsm_status, 2); + if (err) + return err; + + fsm_status = le16_to_cpu(__fsm_status); + if (fsm_status) { + for (i = 0; i < ST_LSM6DSRX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsrx_fsm_sensor_list[i]; + if (!(hw->enable_mask & BIT_ULL(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_lsm6dsrx_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_lsm6dsrx_mlc_probe(struct st_lsm6dsrx_hw *hw) +{ + hw->iio_devs[ST_LSM6DSRX_ID_MLC] = + st_lsm6dsrx_mlc_alloc_iio_dev(hw, ST_LSM6DSRX_ID_MLC); + if (!hw->iio_devs[ST_LSM6DSRX_ID_MLC]) + return -ENOMEM; + + hw->mlc_config = devm_kzalloc(hw->dev, + sizeof(struct st_lsm6dsrx_mlc_config_t), + GFP_KERNEL); + if (!hw->mlc_config) + return -ENOMEM; + + return 0; +} + +int st_lsm6dsrx_mlc_remove(struct device *dev) +{ + struct st_lsm6dsrx_hw *hw = dev_get_drvdata(dev); + + return st_lsm6dsrx_mlc_flush_all(hw); +} +EXPORT_SYMBOL(st_lsm6dsrx_mlc_remove); + +int st_lsm6dsrx_mlc_init_preload(struct st_lsm6dsrx_hw *hw) +{ +#ifdef CONFIG_IIO_ST_LSM6DSRX_MLC_PRELOAD + hw->preload_mlc = 1; + st_lsm6dsrx_mlc_update(&st_lsm6dsrx_mlc_preload, hw); +#endif /* CONFIG_IIO_ST_LSM6DSRX_MLC_PRELOAD */ + + return hw->preload_mlc; +} + diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_preload_mlc.h b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_preload_mlc.h new file mode 100644 index 000000000000..c1cc00098dcc --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_preload_mlc.h @@ -0,0 +1,46 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dsrx mlc preload config + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSRX_PRELOAD_MLC_H +#define ST_LSM6DSRX_PRELOAD_MLC_H + +static const u8 mlcdata[] = { + /* + * fill data from MLC / FSM stored in: + * https://github.com/STMicroelectronics/STMems_Finite_State_Machine + * + * or + * + * https://github.com/STMicroelectronics/STMems_Machine_Learning_Core + * + * based on the needs and type of device (lsm6dsr or lsm6dsrx) + * Follow an example for lsm6dsr FSM free fall detection. + */ + /* lsm6dsr_freefall_detection.ucf */ + 0x01, 0x80, 0x04, 0x00, 0x05, 0x00, 0x5f, 0x0a, 0x46, 0x01, + 0x47, 0x00, 0x0a, 0x00, 0x0b, 0x01, 0x0c, 0x00, 0x0e, 0x00, + 0x0f, 0x00, 0x10, 0x00, 0x02, 0x01, 0x17, 0x40, 0x09, 0x00, + 0x02, 0x11, 0x08, 0x7a, 0x09, 0x00, 0x09, 0x00, 0x09, 0x01, + 0x09, 0x01, 0x09, 0x00, 0x09, 0x04, 0x02, 0x41, 0x08, 0x00, + 0x09, 0x71, 0x09, 0x10, 0x09, 0x1c, 0x09, 0x00, 0x09, 0x11, + 0x09, 0x00, 0x09, 0xcd, 0x09, 0x34, 0x09, 0xa8, 0x09, 0x00, + 0x09, 0x00, 0x09, 0x00, 0x09, 0x01, 0x09, 0x00, 0x09, 0x00, + 0x09, 0x03, 0x09, 0x00, 0x09, 0xf5, 0x09, 0x77, 0x09, 0x99, + 0x09, 0x12, 0x09, 0x66, 0x09, 0x53, 0x09, 0xc7, 0x09, 0x88, + 0x09, 0x99, 0x09, 0x50, 0x09, 0x00, 0x04, 0x00, 0x05, 0x01, + 0x17, 0x00, 0x02, 0x01, 0x01, 0x00, 0x14, 0x80, 0x10, 0x28, + 0x11, 0x00, 0x5e, 0x02, +}; + +static struct firmware st_lsm6dsrx_mlc_preload = { + .size = sizeof(mlcdata), + .data = mlcdata +}; + +#endif /* ST_LSM6DSRX_PRELOAD_MLC_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_shub.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_shub.c new file mode 100644 index 000000000000..97bf31843b17 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_shub.c @@ -0,0 +1,1136 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx sensor hub library driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +#define ST_LSM6DSRX_MAX_SLV_NUM 2 + +/** + * struct st_lsm6dsrx_ext_pwr - External device Power Management description + * @reg: Generic sensor register description. + * @off_val: Value to write into register to power off external sensor. + * @on_val: Value to write into register to power on external sensor. + */ +struct st_lsm6dsrx_ext_pwr { + struct st_lsm6dsrx_reg reg; + u8 off_val; + u8 on_val; +}; + +/** + * struct st_lsm6dsrx_ext_dev_settings - External sensor descriptor entry + * @i2c_addr: External I2C device address (max two). + * @wai_addr: Device ID address. + * @wai_val: Device ID value. + * @odr_table: ODR sensor table. + * @fs_table: Full scale table. + * @temp_comp_reg: Temperature compensation registers. + * @pwr_table: External device Power Management description. + * @off_canc_reg: Offset cancellation registers. + * @bdu_reg: Block Data Update registers. + * @ext_available_scan_masks: IIO device scan mask. + * @ext_channels:IIO device channel specifications. + * @ext_chan_depth: Max number of IIO device channel specifications. + * @data_len: Sensor output data len. + */ +struct st_lsm6dsrx_ext_dev_settings { + u8 i2c_addr[2]; + u8 wai_addr; + u8 wai_val; + struct st_lsm6dsrx_odr_table_entry odr_table; + struct st_lsm6dsrx_fs_table_entry fs_table; + struct st_lsm6dsrx_reg temp_comp_reg; + struct st_lsm6dsrx_ext_pwr pwr_table; + struct st_lsm6dsrx_reg off_canc_reg; + struct st_lsm6dsrx_reg bdu_reg; + unsigned long ext_available_scan_masks[2]; + const struct iio_chan_spec ext_channels[5]; + u8 ext_chan_depth; + u8 data_len; +}; + +static const struct st_lsm6dsrx_ext_dev_settings st_lsm6dsrx_ext_dev_table[] = { + { + /* LIS2MDL */ + .i2c_addr = { 0x1e }, + .wai_addr = 0x4f, + .wai_val = 0x40, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + /* + * added 5Hz for CTS coverage, reg value is the same + * for 5 and 10 Hz + */ + .odr_avl[0] = { 5, 1, 0x0, 0 }, + .odr_avl[1] = { 10, 0, 0x0, 0 }, + .odr_avl[2] = { 20, 0, 0x1, 0 }, + .odr_avl[3] = { 50, 0, 0x2, 0 }, + .odr_avl[4] = { 100, 0, 0x3, 0 }, + }, + .fs_table = { + .size = 1, + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + }, + .temp_comp_reg = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc_reg = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu_reg = { + .addr = 0x62, + .mask = BIT(4), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_LSM6DSRX_DATA_CHANNEL(IIO_MAGN, 0x68, + 1, IIO_MOD_X, 0, + 16, 16, 's', NULL), + .ext_channels[1] = ST_LSM6DSRX_DATA_CHANNEL(IIO_MAGN, 0x6a, + 1, IIO_MOD_Y, 1, + 16, 16, 's', NULL), + .ext_channels[2] = ST_LSM6DSRX_DATA_CHANNEL(IIO_MAGN, 0x6c, + 1, IIO_MOD_Z, 2, + 16, 16, 's', NULL), + .ext_channels[3] = ST_LSM6DSRX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + { + /* LIS3MDL */ + .i2c_addr = { 0x1c, 0x1e }, + .wai_addr = 0x0f, + .wai_val = 0x3d, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x20, + .mask = GENMASK(4, 2), + }, + .odr_avl[0] = { 5, 0, 0x3, 0 }, + .odr_avl[1] = { 10, 0, 0x3, 0 }, + .odr_avl[2] = { 20, 0, 0x4, 0 }, + .odr_avl[3] = { 40, 0, 0x5, 0 }, + .odr_avl[4] = { 80, 0, 0x6, 0 }, + .odr_avl[5] = { 100, 0, 0x7, 0 }, + }, + .fs_table = { + .size = 4, + .fs_avl[0] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 6842, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 3421, + .val = 0x1, + }, + .fs_avl[2] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 2281, + .val = 0x2, + }, + .fs_avl[3] = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .gain = 1711, + .val = 0x3, + }, + }, + .temp_comp_reg = { + .addr = 0x20, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x22, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .bdu_reg = { + .addr = 0x24, + .mask = BIT(6), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_LSM6DSRX_DATA_CHANNEL(IIO_MAGN, 0x28, + 1, IIO_MOD_X, 0, + 16, 16, 's', NULL), + .ext_channels[1] = ST_LSM6DSRX_DATA_CHANNEL(IIO_MAGN, 0x2a, + 1, IIO_MOD_Y, 1, + 16, 16, 's', NULL), + .ext_channels[2] = ST_LSM6DSRX_DATA_CHANNEL(IIO_MAGN, 0x2c, + 1, IIO_MOD_Z, 2, + 16, 16, 's', NULL), + .ext_channels[3] = ST_LSM6DSRX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + { + /* LPS22HB */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb1, + .odr_table = { + .size = 4, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1, 0 }, + .odr_avl[1] = { 10, 0, 0x2, 0 }, + .odr_avl[2] = { 25, 0, 0x3, 0 }, + .odr_avl[3] = { 50, 0, 0x4, 0 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_LSM6DSRX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u', NULL), + .ext_channels[1] = ST_LSM6DSRX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, + { + /* LPS22HH */ + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb3, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 0, 0x1, 0 }, + .odr_avl[1] = { 10, 0, 0x2, 0 }, + .odr_avl[2] = { 25, 0, 0x3, 0 }, + .odr_avl[3] = { 50, 0, 0x4, 0 }, + .odr_avl[4] = { 100, 0, 0x6, 0 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_LSM6DSRX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u', NULL), + .ext_channels[1] = ST_LSM6DSRX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, +}; + +/** + * st_lsm6dsrx_shub_wait_complete() - Wait write trigger [SHUB] + * + * @hw: ST IMU MEMS hw instance. + * + * In write on external device register, each operation is triggered + * by accel/gyro data ready, this means that wait time depends on ODR + * plus i2c time. + * NOTE: Be sure to enable Acc or Gyro before this operation. + */ +static inline void st_lsm6dsrx_shub_wait_complete(struct st_lsm6dsrx_hw *hw) +{ + struct st_lsm6dsrx_sensor *sensor; + int odr, uodr; + + sensor = iio_priv(hw->iio_devs[ST_LSM6DSRX_ID_ACC]); + + /* check if acc is enabled (it should be) */ + if (hw->enable_mask & BIT_ULL(ST_LSM6DSRX_ID_ACC)) { + odr = sensor->odr; + uodr = sensor->uodr; + } else { + odr = 12; + uodr = 500000; + } + + msleep((2000000000U / (odr * 1000000 + uodr)) + 1); +} + +/** + * st_lsm6dsrx_shub_read_reg() - Read from sensor hub register [SHUB] + * + * @hw: ST IMU MEMS hw instance. + * @addr: Remote address register. + * @data: Data buffer. + * @len: Data read len. + * + * return 0 if OK, < 0 if ERROR. + * + * NOTE: uses page_lock. + */ +static int st_lsm6dsrx_shub_read_reg(struct st_lsm6dsrx_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, true, + ST_LSM6DSRX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = regmap_bulk_read(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dsrx_set_page_access(hw, false, + ST_LSM6DSRX_REG_SHUB_REG_MASK); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_shub_write_reg() - Write to sensor hub register [SHUB] + * + * @hw: ST IMU MEMS hw instance. + * @addr: Remote address register. + * @data: Data buffer. + * @len: Data read len. + * + * return 0 if OK, < 0 if ERROR. + * + * NOTE: uses page_lock. + */ +static int st_lsm6dsrx_shub_write_reg(struct st_lsm6dsrx_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, true, + ST_LSM6DSRX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = regmap_bulk_write(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dsrx_set_page_access(hw, false, + ST_LSM6DSRX_REG_SHUB_REG_MASK); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_shub_master_enable() - Enable sensor hub interface [SHUB] + * + * @sensor: ST IMU sensor instance. + * @enable: Master Enable/Disable. + * + * return 0 if OK, < 0 if ERROR. + * + * NOTE: uses page_lock. + */ +static int st_lsm6dsrx_shub_master_enable(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsrx_hw *hw = sensor->hw; + int err; + + /* enable acc sensor as trigger */ + err = st_lsm6dsrx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsrx_set_page_access(hw, true, + ST_LSM6DSRX_REG_SHUB_REG_MASK); + if (err < 0) + goto out; + + err = __st_lsm6dsrx_write_with_mask(hw, + ST_LSM6DSRX_REG_MASTER_CONFIG_ADDR, + ST_LSM6DSRX_REG_MASTER_ON_MASK, + enable); + + st_lsm6dsrx_set_page_access(hw, false, ST_LSM6DSRX_REG_SHUB_REG_MASK); + +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsrx_shub_read() - Read sensor data register from shub + * + * @sensor: ST IMU sensor instance. + * @addr: Remote address register. + * @data: Data buffer. + * @len: Data read len. + * + * return 0 if OK, < 0 if ERROR. + * + * NOTE: use SLV3 i2c slave for one-shot read operation. + */ +static int st_lsm6dsrx_shub_read(struct st_lsm6dsrx_sensor *sensor, u8 addr, + u8 *data, int len) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsrx_hw *hw = sensor->hw; + u8 out_addr = ST_LSM6DSRX_REG_SLV0_OUT_ADDR + hw->ext_data_len; + u8 config[3]; + int err; + + config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[1] = addr; + config[2] = len & 0x7; + + err = st_lsm6dsrx_shub_write_reg(hw, ST_LSM6DSRX_REG_SLV3_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsrx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsrx_shub_wait_complete(hw); + + err = st_lsm6dsrx_shub_read_reg(hw, out_addr, data, len & 0x7); + + st_lsm6dsrx_shub_master_enable(sensor, false); + + memset(config, 0, sizeof(config)); + + return st_lsm6dsrx_shub_write_reg(hw, ST_LSM6DSRX_REG_SLV3_ADDR, + config, sizeof(config)); +} + +/** + * st_lsm6dsrx_shub_write() - Write sensor data register from shub + * + * @sensor: ST IMU sensor instance. + * @addr: Remote address register. + * @data: Data buffer. + * @len: Data read len. + * + * return 0 if OK, < 0 if ERROR. + * + * NOTE: use SLV0 i2c slave for write operation. + */ +static int st_lsm6dsrx_shub_write(struct st_lsm6dsrx_sensor *sensor, u8 addr, + u8 *data, int len) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsrx_hw *hw = sensor->hw; + u8 mconfig = ST_LSM6DSRX_REG_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + u8 config[3] = {}; + int err, i; + + /* AuxSens = 3 + wr once + pull up configuration */ + err = st_lsm6dsrx_shub_write_reg(hw, + ST_LSM6DSRX_REG_MASTER_CONFIG_ADDR, + &mconfig, sizeof(mconfig)); + if (err < 0) + return err; + + config[0] = ext_info->ext_dev_i2c_addr << 1; + for (i = 0; i < len; i++) { + config[1] = addr + i; + + err = st_lsm6dsrx_shub_write_reg(hw, + ST_LSM6DSRX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsrx_shub_write_reg(hw, + ST_LSM6DSRX_REG_DATAWRITE_SLV0_ADDR, + &data[i], 1); + if (err < 0) + return err; + + err = st_lsm6dsrx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsrx_shub_wait_complete(hw); + + st_lsm6dsrx_shub_master_enable(sensor, false); + } + + return st_lsm6dsrx_shub_write_reg(hw, ST_LSM6DSRX_REG_SLV0_ADDR, + config, sizeof(config)); +} + +/** + * st_lsm6dsrx_shub_write_with_mask() - Write sensor data register from + * sensor hub interface using + * register bitmask + * + * @sensor: ST IMU sensor instance. + * @addr: Remote address register. + * @mask: Register bitmask. + * @val: Data buffer. + * + * return 0 if OK, < 0 if ERROR. + */ +static int st_lsm6dsrx_shub_write_with_mask(struct st_lsm6dsrx_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_lsm6dsrx_shub_read(sensor, addr, &data, sizeof(data)); + if (err < 0) + return err; + + data = (data & ~mask) | ST_LSM6DSRX_SHIFT_VAL(val, mask); + + return st_lsm6dsrx_shub_write(sensor, addr, &data, sizeof(data)); +} + +/** + * st_lsm6dsrx_shub_config_channels() - Configure external sensor + * connected on master I2C + * + * @sensor: ST IMU sensor instance. + * @enable: Enable/Disable sensor. + * + * return 0 if OK, < 0 if ERROR. + * + * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation. + */ +static int st_lsm6dsrx_shub_config_channels(struct st_lsm6dsrx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info; + struct st_lsm6dsrx_hw *hw = sensor->hw; + struct st_lsm6dsrx_sensor *cur_sensor; + u8 config[6] = {}, enable_mask; + int i, j = 0; + + enable_mask = enable ? hw->enable_mask | BIT_ULL(sensor->id) + : hw->enable_mask & ~BIT_ULL(sensor->id); + + for (i = ST_LSM6DSRX_ID_EXT0; i <= ST_LSM6DSRX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT_ULL(cur_sensor->id))) + continue; + + ext_info = &cur_sensor->ext_dev_info; + config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[j + 1] = + ext_info->ext_dev_settings->ext_channels[0].address; + config[j + 2] = ST_LSM6DSRX_REG_BATCH_EXT_SENS_EN_MASK | + (ext_info->ext_dev_settings->data_len & + ST_LSM6DSRX_REG_SLAVE_NUMOP_MASK); + j += 3; + } + + return st_lsm6dsrx_shub_write_reg(hw, ST_LSM6DSRX_REG_SLV1_ADDR, + config, sizeof(config)); +} + +/** + * st_lsm6dsrx_shub_get_odr_val() - Get a valid ODR [SHUB] + * + * @sensor: SST IMU sensor instance. + * @odr: ODR value (in Hz). + * @val: ODR register value data pointer. + * + * Return a valid ODR register value closest to the passed value. + * + * return 0 if OK, negative value for ERROR. + */ +static int st_lsm6dsrx_shub_get_odr_val(struct st_lsm6dsrx_sensor *sensor, + u16 odr, u8 *val) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) + if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr) + break; + + if (i == ext_info->ext_dev_settings->odr_table.size) + return -EINVAL; + + *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val; + + /* set decimator for low ODR */ + sensor->decimator = + ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz; + sensor->dec_counter = 0; + + return 0; +} + +/** + * st_lsm6dsrx_shub_set_odr() - Set new ODR to sensor [SHUB] + * + * @sensor: ST IMU sensor instance. + * @odr: ODR value (in Hz). + * + * Set a valid ODR closest to the passed value. + * + * return 0 if OK, negative value for ERROR. + */ +static int st_lsm6dsrx_shub_set_odr(struct st_lsm6dsrx_sensor *sensor, u16 odr) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsrx_hw *hw = sensor->hw; + u8 odr_val; + int err; + + err = st_lsm6dsrx_shub_get_odr_val(sensor, odr, &odr_val); + if (err < 0) + return err; + + if (sensor->odr == odr && (hw->enable_mask & + BIT_ULL(sensor->id))) + return 0; + + return st_lsm6dsrx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + odr_val); +} + +/** + * st_lsm6dsrx_shub_set_enable() - Enable or Disable sensor [SHUB] + * + * @sensor: ST IMU sensor instance. + * @enable: Enable or disable the sensor [true, false]. + * + * return 0 if OK, negative value for ERROR. + */ +int st_lsm6dsrx_shub_set_enable(struct st_lsm6dsrx_sensor *sensor, bool enable) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err; + + err = st_lsm6dsrx_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + if (enable) { + err = st_lsm6dsrx_shub_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + } else { + err = st_lsm6dsrx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + 0); + if (err < 0) + return err; + } + + if (ext_info->ext_dev_settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? ext_info->ext_dev_settings->pwr_table.on_val + : ext_info->ext_dev_settings->pwr_table.off_val; + err = st_lsm6dsrx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->pwr_table.reg.addr, + ext_info->ext_dev_settings->pwr_table.reg.mask, + val); + if (err < 0) + return err; + } + + return st_lsm6dsrx_shub_master_enable(sensor, enable); +} + +static inline u32 st_lsm6dsrx_get_unaligned_le24(const u8 *p) +{ + return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; +} + +/** + * st_lsm6dsrx_shub_read_oneshot() - Single sensor read operation [SHUB] + * + * @sensor: ST IMU sensor instance. + * @ch: IIO Channel. + * @val: Output data register value. + * + * return IIO_VAL_INT if OK, negative value for ERROR. + */ +static int st_lsm6dsrx_shub_read_oneshot(struct st_lsm6dsrx_sensor *sensor, + struct iio_chan_spec const *ch, + int *val) +{ + int err, delay, len = ch->scan_type.realbits >> 3; + u8 data[4]; + + if (len > ARRAY_SIZE(data)) + return -ENOMEM; + + err = st_lsm6dsrx_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsrx_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + st_lsm6dsrx_shub_set_enable(sensor, false); + + switch (len) { + case 3: + *val = (s32)st_lsm6dsrx_get_unaligned_le24(data); + break; + case 2: + *val = (s16)get_unaligned_le16(data); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +/** + * st_lsm6dsrx_shub_read_raw() - Read Sensor data configuration [SHUB] + * + * @iio_dev: IIO Device. + * @ch: IIO Channel. + * @val: Data Buffer (MSB). + * @val2: Data Buffer (LSB). + * @mask: Data Mask. + * + * return 0 if OK, -EINVAL value for ERROR. + */ +static int st_lsm6dsrx_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsrx_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_lsm6dsrx_shub_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * st_lsm6dsrx_shub_write_raw() - Write Sensor data configuration [SHUB] + * + * @iio_dev: IIO Device. + * @chan: IIO Channel. + * @val: Data Buffer (MSB). + * @val2: Data Buffer (LSB). + * @mask: Data Mask. + * + * return 0 if OK, -EINVAL value for ERROR. + */ +static int st_lsm6dsrx_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_lsm6dsrx_shub_get_odr_val(sensor, val, &data); + if (!err) + sensor->odr = val; + break; + } + case IIO_CHAN_INFO_SCALE: + err = 0; + break; + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * st_lsm6dsrx_sysfs_shub_sampling_freq_avail() - Get a list of + * available sensor ODR + * + * @dev: IIO Device. + * @attr: IIO Channel attribute. + * @buf: User buffer. + * + * List of available ODR returned separated by commas. + * + * return buffer len. + */ +static ssize_t +st_lsm6dsrx_sysfs_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) { + u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +/** + * st_lsm6dsrx_sysfs_shub_scale_avail() - Get a list of available sensor + * Full Scale + * + * List of available Full Scale returned separated by commas. + * + * @dev: IIO Device. + * @attr: IIO Channel attribute. + * @buf: User buffer. + * + * return buffer len. + */ +static ssize_t st_lsm6dsrx_sysfs_shub_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsrx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->fs_table.size; i++) { + u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + val); + } + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsrx_sysfs_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_ext_scale_available, 0444, + st_lsm6dsrx_sysfs_shub_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsrx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsrx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsrx_get_watermark, + st_lsm6dsrx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsrx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsrx_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_ext_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_lsm6dsrx_ext_attribute_group = { + .attrs = st_lsm6dsrx_ext_attributes, +}; + +static const struct iio_info st_lsm6dsrx_ext_info = { + .attrs = &st_lsm6dsrx_ext_attribute_group, + .read_raw = st_lsm6dsrx_shub_read_raw, + .write_raw = st_lsm6dsrx_shub_write_raw, +}; + +/** + * st_lsm6dsrx_shub_alloc_iio_dev() - Allocate IIO device [SHUB] + * + * @hw: ST IMU MEMS hw instance. + * @ext_settings: external sensor descritor entry. + * @id: Sensor Identifier. + * @i2c_addr: external I2C address on master bus. + * + * return struct iio_dev *, NULL if ERROR. + */ +static struct iio_dev *st_lsm6dsrx_shub_alloc_iio_dev(struct st_lsm6dsrx_hw *hw, + const struct st_lsm6dsrx_ext_dev_settings *ext_settings, + enum st_lsm6dsrx_sensor_id id, u8 i2c_addr) +{ + struct st_lsm6dsrx_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; + iio_dev->available_scan_masks = ext_settings->ext_available_scan_masks; + iio_dev->info = &st_lsm6dsrx_ext_info; + iio_dev->channels = ext_settings->ext_channels; + iio_dev->num_channels = ext_settings->ext_chan_depth; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = ext_settings->odr_table.odr_avl[0].hz; + sensor->gain = ext_settings->fs_table.fs_avl[0].gain; + sensor->max_watermark = ST_LSM6DSRX_MAX_FIFO_DEPTH; + sensor->watermark = 1; + sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr; + sensor->ext_dev_info.ext_dev_settings = ext_settings; + sensor->decimator = 0; + sensor->dec_counter = 0; + sensor->pm = ST_LSM6DSRX_NO_MODE; + + switch (iio_dev->channels[0].type) { + case IIO_MAGN: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_magn", hw->settings->id.name); + break; + case IIO_PRESSURE: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", hw->settings->id.name); + break; + default: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ext", hw->settings->id.name); + break; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static int st_lsm6dsrx_shub_init_remote_sensor(struct st_lsm6dsrx_sensor *sensor) +{ + struct st_lsm6dsrx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err = 0; + + if (ext_info->ext_dev_settings->bdu_reg.addr) + err = st_lsm6dsrx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->bdu_reg.addr, + ext_info->ext_dev_settings->bdu_reg.mask, 1); + + if (ext_info->ext_dev_settings->temp_comp_reg.addr) + err = st_lsm6dsrx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->temp_comp_reg.addr, + ext_info->ext_dev_settings->temp_comp_reg.mask, 1); + + if (ext_info->ext_dev_settings->off_canc_reg.addr) + err = st_lsm6dsrx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->off_canc_reg.addr, + ext_info->ext_dev_settings->off_canc_reg.mask, 1); + + return err; +} + +/** + * st_lsm6dsrx_shub_probe() - Probe device function [SHUB] + * + * @hw: ST IMU MEMS hw instance. + * + * return 0 if OK, negative for ERROR. + */ +int st_lsm6dsrx_shub_probe(struct st_lsm6dsrx_hw *hw) +{ + const struct st_lsm6dsrx_ext_dev_settings *settings; + struct st_lsm6dsrx_sensor *acc_sensor, *sensor; + u8 config[3], data, num_ext_dev = 0; + enum st_lsm6dsrx_sensor_id id; + int err, i = 0, j; + struct device_node *np = hw->dev->of_node; + + if (np && of_property_read_bool(np, "drive-pullup-shub")) { + dev_err(hw->dev, "enabling pull up on i2c master\n"); + err = st_lsm6dsrx_shub_read_reg(hw, + ST_LSM6DSRX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + if (err < 0) + return err; + + data |= ST_LSM6DSRX_REG_SHUB_PU_EN_MASK; + err = st_lsm6dsrx_shub_write_reg(hw, + ST_LSM6DSRX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); + + if (err < 0) + return err; + + hw->i2c_master_pu = ST_LSM6DSRX_REG_SHUB_PU_EN_MASK; + } + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSRX_ID_ACC]); + while (i < ARRAY_SIZE(st_lsm6dsrx_ext_dev_table) && + num_ext_dev < ST_LSM6DSRX_MAX_SLV_NUM) { + settings = &st_lsm6dsrx_ext_dev_table[i]; + + for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) { + if (!settings->i2c_addr[j]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[j] << 1) | 1; + config[1] = settings->wai_addr; + config[2] = 1; + + err = st_lsm6dsrx_shub_write_reg(hw, + ST_LSM6DSRX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsrx_shub_master_enable(acc_sensor, + true); + if (err < 0) + return err; + + st_lsm6dsrx_shub_wait_complete(hw); + + err = st_lsm6dsrx_shub_read_reg(hw, + ST_LSM6DSRX_REG_SLV0_OUT_ADDR, + &data, sizeof(data)); + + st_lsm6dsrx_shub_master_enable(acc_sensor, false); + + if (err < 0) + return err; + + if (data != settings->wai_val) + continue; + + id = ST_LSM6DSRX_ID_EXT0 + num_ext_dev; + hw->iio_devs[id] = st_lsm6dsrx_shub_alloc_iio_dev(hw, + settings, id, + settings->i2c_addr[j]); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_lsm6dsrx_shub_init_remote_sensor(sensor); + if (err < 0) + return err; + + num_ext_dev++; + hw->ext_data_len += settings->data_len; + break; + } + + i++; + } + + if (!num_ext_dev) + return 0; + + memset(config, 0, sizeof(config)); + err = st_lsm6dsrx_shub_write_reg(hw, ST_LSM6DSRX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + /* AuxSens = 3 + wr once */ + data = ST_LSM6DSRX_REG_WRITE_ONCE_MASK | 3 | hw->i2c_master_pu; + return st_lsm6dsrx_shub_write_reg(hw, + ST_LSM6DSRX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_spi.c b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_spi.c new file mode 100644 index 000000000000..66643666aa0f --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsrx/st_lsm6dsrx_spi.c @@ -0,0 +1,78 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsrx spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsrx.h" + +static const struct regmap_config st_lsm6dsrx_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsrx_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_lsm6dsrx_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_lsm6dsrx_probe(&spi->dev, spi->irq, hw_id, regmap); +} + +static int st_lsm6dsrx_spi_remove(struct spi_device *spi) +{ + return st_lsm6dsrx_mlc_remove(&spi->dev); +} + +static const struct of_device_id st_lsm6dsrx_spi_of_match[] = { + { + .compatible = "st," ST_LSM6DSRX_DEV_NAME, + .data = (void *)ST_LSM6DSRX_ID, + }, + { + .compatible = "st," ST_LSM6DSR_DEV_NAME, + .data = (void *)ST_LSM6DSR_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsrx_spi_of_match); + +static const struct spi_device_id st_lsm6dsrx_spi_id_table[] = { + { ST_LSM6DSRX_DEV_NAME, ST_LSM6DSRX_ID }, + { ST_LSM6DSR_DEV_NAME, ST_LSM6DSR_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsrx_spi_id_table); + +static struct spi_driver st_lsm6dsrx_driver = { + .driver = { + .name = "st_" ST_LSM6DSRX_DEV_NAME "_spi", + .pm = &st_lsm6dsrx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsrx_spi_of_match), + }, + .probe = st_lsm6dsrx_spi_probe, + .remove = st_lsm6dsrx_spi_remove, + .id_table = st_lsm6dsrx_spi_id_table, +}; +module_spi_driver(st_lsm6dsrx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsrx spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/Kconfig b/drivers/iio/stm/imu/st_lsm6dsvx/Kconfig new file mode 100644 index 000000000000..6901b6b97cf2 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/Kconfig @@ -0,0 +1,51 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM6DSVX + tristate "STMicroelectronics LSM6DSVX sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_LSM6DSVX_I2C if (I2C) + select IIO_ST_LSM6DSVX_SPI if (SPI_MASTER) + select IIO_ST_LSM6DSVX_I3C if (I3C) + help + Say yes here to build support for STMicroelectronics LSM6DSVX imu + sensor. + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsvx. + +config IIO_ST_LSM6DSVX_I2C + tristate + depends on IIO_ST_LSM6DSVX + +config IIO_ST_LSM6DSVX_SPI + tristate + depends on IIO_ST_LSM6DSVX + +config IIO_ST_LSM6DSVX_I3C + tristate + depends on IIO_ST_LSM6DSVX + select REGMAP_I3C + +config IIO_ST_LSM6DSVX_QVAR_IN_FIFO + bool "use FIFO for QVAR" + depends on IIO_ST_LSM6DSVX + help + Enable support to QVAR sensor on internal HW FIFO + +config IIO_ST_LSM6DSVX_MLC_PRELOAD + bool "Preload some examples on MLC/FSM core" + depends on IIO_ST_LSM6DSVX + 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_LSM6DSVX_ASYNC_HW_TIMESTAMP + bool "Enable async hw timestamp read" + depends on IIO_ST_LSM6DSVX + help + Enable async task that sends over hw timestamp events. diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/Makefile b/drivers/iio/stm/imu/st_lsm6dsvx/Makefile new file mode 100644 index 000000000000..6da0fe88bc07 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/Makefile @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_lsm6dsvx-y := st_lsm6dsvx_core.o st_lsm6dsvx_buffer.o \ + st_lsm6dsvx_shub.o st_lsm6dsvx_qvar.o \ + st_lsm6dsvx_mlc.o st_lsm6dsvx_events.o \ + st_lsm6dsvx_embfunc.o + +st_lsm6dsvx-$(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) += st_lsm6dsvx_hwtimestamp.o + +obj-$(CONFIG_IIO_ST_LSM6DSVX) += st_lsm6dsvx.o +obj-$(CONFIG_IIO_ST_LSM6DSVX_I2C) += st_lsm6dsvx_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSVX_SPI) += st_lsm6dsvx_spi.o +obj-$(CONFIG_IIO_ST_LSM6DSVX_I3C) += st_lsm6dsvx_i3c.o +obj-$(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) += st_lsm6dsvx_hwtimestamp.o diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx.h b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx.h new file mode 100644 index 000000000000..82205ce35197 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx.h @@ -0,0 +1,1019 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dsvx sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSVX_H +#define ST_LSM6DSVX_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#define ST_LSM6DSVX_ODR_LIST_SIZE 9 +#define ST_LSM6DSVX_ODR_EXPAND(odr, uodr) ((odr * 1000000) + uodr) + +#define ST_LSM6DSV16X_DEV_NAME "lsm6dsv16x" +#define ST_LSM6DSV_DEV_NAME "lsm6dsv" + +#define ST_LSM6DSVX_SAMPLE_SIZE 6 +#define ST_LSM6DSVX_TS_SAMPLE_SIZE 4 +#define ST_LSM6DSVX_TAG_SIZE 1 +#define ST_LSM6DSVX_FIFO_SAMPLE_SIZE (ST_LSM6DSVX_SAMPLE_SIZE + \ + ST_LSM6DSVX_TAG_SIZE) +#define ST_LSM6DSVX_MAX_FIFO_DEPTH 208 + +/* register map */ +#define ST_LSM6DSVX_REG_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DSVX_SHUB_REG_ACCESS_MASK BIT(6) +#define ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK BIT(7) + +#define ST_LSM6DSVX_REG_IF_CFG_ADDR 0x03 +#define ST_LSM6DSVX_PP_OD_MASK BIT(3) +#define ST_LSM6DSVX_H_LACTIVE_MASK BIT(4) +#define ST_LSM6DSVX_SHUB_PU_EN_MASK BIT(6) + +#define ST_LSM6DSVX_REG_FIFO_CTRL1_ADDR 0x07 +#define ST_LSM6DSVX_WTM_MASK GENMASK(7, 0) + +#define ST_LSM6DSVX_REG_FIFO_CTRL3_ADDR 0x09 +#define ST_LSM6DSVX_BDR_XL_MASK GENMASK(3, 0) +#define ST_LSM6DSVX_BDR_GY_MASK GENMASK(7, 4) + +#define ST_LSM6DSVX_REG_FIFO_CTRL4_ADDR 0x0a +#define ST_LSM6DSVX_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_LSM6DSVX_ODR_T_BATCH_MASK GENMASK(5, 4) +#define ST_LSM6DSVX_DEC_TS_BATCH_MASK GENMASK(7, 6) + +#define ST_LSM6DSVX_REG_INT1_CTRL_ADDR 0x0d +#define ST_LSM6DSVX_REG_INT2_CTRL_ADDR 0x0e +#define ST_LSM6DSVX_INT_FIFO_TH_MASK BIT(3) + +#define ST_LSM6DSVX_REG_WHOAMI_ADDR 0x0f +#define ST_LSM6DSVX_WHOAMI_VAL 0x70 + +#define ST_LSM6DSVX_REG_CTRL1_ADDR 0x10 +#define ST_LSM6DSVX_REG_CTRL2_ADDR 0x11 + +#define ST_LSM6DSVX_REG_CTRL3_ADDR 0x12 +#define ST_LSM6DSVX_SW_RESET_MASK BIT(0) +#define ST_LSM6DSVX_BDU_MASK BIT(6) +#define ST_LSM6DSVX_BOOT_MASK BIT(7) + +#define ST_LSM6DSVX_REG_CTRL4_ADDR 0x13 +#define ST_LSM6DSVX_DRDY_MASK BIT(3) + +#define ST_LSM6DSVX_REG_CTRL6_ADDR 0x15 + +#define ST_LSM6DSVX_REG_CTRL7_ADDR 0x16 +#define ST_LSM6DSVX_AH_QVAR_C_ZIN_MASK GENMASK(5, 4) +#define ST_LSM6DSVX_AH_QVAR_EN_MASK BIT(7) + +#define ST_LSM6DSVX_REG_CTRL8_ADDR 0x17 + +#define ST_LSM6DSVX_REG_CTRL10_ADDR 0x19 +#define ST_LSM6DSVX_ST_XL_MASK GENMASK(1, 0) +#define ST_LSM6DSVX_ST_G_MASK GENMASK(3, 2) + +#define ST_LSM6DSVX_REG_FIFO_STATUS1_ADDR 0x1b +#define ST_LSM6DSVX_FIFO_DIFF_MASK GENMASK(8, 0) + +#define ST_LSM6DSVX_REG_ALL_INT_SRC_ADDR 0x1d +#define ST_LSM6DSVX_FF_IA_MASK BIT(0) +#define ST_LSM6DSVX_WU_IA_MASK BIT(1) +#define ST_LSM6DSVX_TAP_IA_MASK BIT(2) +#define ST_LSM6DSVX_D6D_IA_MASK BIT(4) +#define ST_LSM6DSVX_SLEEP_CHANGE_MASK BIT(5) + +#define ST_LSM6DSVX_REG_D6D_SRC_ADDR 0x1d +#define ST_LSM6DSVX_D6D_EVENT_MASK GENMASK(5, 0) + +#define ST_LSM6DSVX_REG_STATUS_REG_ADDR 0x1e +#define ST_LSM6DSVX_XLDA_MASK BIT(0) +#define ST_LSM6DSVX_GDA_MASK BIT(1) +#define ST_LSM6DSVX_TDA_MASK BIT(2) + +#define ST_LSM6DSVX_REG_OUT_TEMP_L_ADDR 0x20 + +#define ST_LSM6DSVX_REG_OUTX_L_G_ADDR 0x22 +#define ST_LSM6DSVX_REG_OUTY_L_G_ADDR 0x24 +#define ST_LSM6DSVX_REG_OUTZ_L_G_ADDR 0x26 +#define ST_LSM6DSVX_REG_OUTX_L_A_ADDR 0x28 +#define ST_LSM6DSVX_REG_OUTY_L_A_ADDR 0x2a +#define ST_LSM6DSVX_REG_OUTZ_L_A_ADDR 0x2c + +#define ST_LSM6DSVX_REG_OUT_QVAR_ADDR 0x3a + +#define ST_LSM6DSVX_REG_TIMESTAMP0_ADDR 0x40 +#define ST_LSM6DSVX_REG_TIMESTAMP2_ADDR 0x42 + +#define ST_LSM6DSVX_REG_WAKE_UP_SRC_ADDR 0x45 +#define ST_LSM6DSVX_WAKE_UP_EVENT_MASK GENMASK(3, 0) + +#define ST_LSM6DSVX_REG_EMB_FUNC_STATUS_MAINPAGE_ADDR 0x49 +#define ST_LSM6DSVX_IS_STEP_DET_MASK BIT(3) +#define ST_LSM6DSVX_IS_TILT_MASK BIT(4) +#define ST_LSM6DSVX_IS_SIGMOT_MASK BIT(5) + +#define ST_LSM6DSVX_REG_FSM_STATUS_MAINPAGE_ADDR 0x4a +#define ST_LSM6DSVX_REG_MLC_STATUS_MAINPAGE_ADDR 0x4b + +#define ST_LSM6DSVX_REG_INTERNAL_FREQ_FINE 0x4f + +#define ST_LSM6DSVX_REG_FUNCTIONS_ENABLE_ADDR 0x50 +#define ST_LSM6DSVX_TIMESTAMP_EN_MASK BIT(6) +#define ST_LSM6DSVX_INTERRUPTS_ENABLE_MASK BIT(7) + +#define ST_LSM6DSVX_REG_TAP_CFG0_ADDR 0x56 +#define ST_LSM6DSVX_LIR_MASK BIT(0) +#define ST_LSM6DSVX_REG_TAP_Z_EN_MASK BIT(1) +#define ST_LSM6DSVX_REG_TAP_Y_EN_MASK BIT(2) +#define ST_LSM6DSVX_REG_TAP_X_EN_MASK BIT(3) +#define ST_LSM6DSVX_REG_TAP_EN_MASK GENMASK(3, 1) + +#define ST_LSM6DSVX_REG_TAP_CFG1_ADDR 0x57 +#define ST_LSM6DSVX_TAP_THS_X_MASK GENMASK(4, 0) +#define ST_LSM6DSVX_TAP_PRIORITY_MASK GENMASK(7, 5) + +#define ST_LSM6DSVX_REG_TAP_CFG2_ADDR 0x58 +#define ST_LSM6DSVX_TAP_THS_Y_MASK GENMASK(4, 0) + +#define ST_LSM6DSVX_REG_TAP_THS_6D_ADDR 0x59 +#define ST_LSM6DSVX_TAP_THS_Z_MASK GENMASK(4, 0) +#define ST_LSM6DSVX_SIXD_THS_MASK GENMASK(6, 5) + +#define ST_LSM6DSVX_REG_TAP_DUR_ADDR 0x5a +#define ST_LSM6DSVX_SHOCK_MASK GENMASK(1, 0) +#define ST_LSM6DSVX_QUIET_MASK GENMASK(3, 2) +#define ST_LSM6DSVX_DUR_MASK GENMASK(7, 4) + +#define ST_LSM6DSVX_REG_WAKE_UP_THS_ADDR 0x5b +#define ST_LSM6DSVX_WK_THS_MASK GENMASK(5, 0) +#define ST_LSM6DSVX_SINGLE_DOUBLE_TAP_MASK BIT(7) + +#define ST_LSM6DSVX_REG_WAKE_UP_DUR_ADDR 0x5c +#define ST_LSM6DSVX_WAKE_DUR_MASK GENMASK(6, 5) + +#define ST_LSM6DSVX_REG_FREE_FALL_ADDR 0x5d +#define ST_LSM6DSVX_FF_THS_MASK GENMASK(2, 0) + +#define ST_LSM6DSVX_REG_MD1_CFG_ADDR 0x5e +#define ST_LSM6DSVX_REG_MD2_CFG_ADDR 0x5f +#define ST_LSM6DSVX_REG_INT2_TIMESTAMP_MASK BIT(0) +#define ST_LSM6DSVX_REG_INT_EMB_FUNC_MASK BIT(1) +#define ST_LSM6DSVX_INT_6D_MASK BIT(2) +#define ST_LSM6DSVX_INT_DOUBLE_TAP_MASK BIT(3) +#define ST_LSM6DSVX_INT_FF_MASK BIT(4) +#define ST_LSM6DSVX_INT_WU_MASK BIT(5) +#define ST_LSM6DSVX_INT_SINGLE_TAP_MASK BIT(6) +#define ST_LSM6DSVX_INT_SLEEP_CHANGE_MASK BIT(7) + +#define ST_LSM6DSVX_REG_FIFO_DATA_OUT_TAG_ADDR 0x78 + +/* embedded function registers */ +#define ST_LSM6DSVX_REG_PAGE_SEL_ADDR 0x02 + +#define ST_LSM6DSVX_REG_EMB_FUNC_EN_A_ADDR 0x04 +#define ST_LSM6DSVX_SFLP_GAME_EN_MASK BIT(1) +#define ST_LSM6DSVX_REG_PEDO_EN_MASK BIT(3) +#define ST_LSM6DSVX_REG_TILT_EN_MASK BIT(4) +#define ST_LSM6DSVX_REG_SIGN_MOTION_EN_MASK BIT(5) + +#define ST_LSM6DSVX_REG_EMB_FUNC_EN_B_ADDR 0x05 +#define ST_LSM6DSVX_FSM_EN_MASK BIT(0) +#define ST_LSM6DSVX_MLC_EN_MASK BIT(4) + +#define ST_LSM6DSVX_REG_PAGE_ADDRESS_ADDR 0x08 +#define ST_LSM6DSVX_REG_PAGE_VALUE_ADDR 0x09 + +#define ST_LSM6DSVX_REG_EMB_FUNC_INT1_ADDR 0x0a +#define ST_LSM6DSVX_INT_STEP_DETECTOR_MASK BIT(3) +#define ST_LSM6DSVX_INT_TILT_MASK BIT(4) +#define ST_LSM6DSVX_INT_SIG_MOT_MASK BIT(5) + +#define ST_LSM6DSVX_REG_FSM_INT1_ADDR 0x0b +#define ST_LSM6DSVX_REG_MLC_INT1_ADDR 0x0d +#define ST_LSM6DSVX_REG_EMB_FUNC_INT2_ADDR 0x0e +#define ST_LSM6DSVX_REG_FSM_INT2_ADDR 0x0f +#define ST_LSM6DSVX_REG_MLC_INT2_ADDR 0x11 + +#define ST_LSM6DSVX_REG_FSM_STATUS_ADDR 0x13 +#define ST_LSM6DSVX_REG_MLC_STATUS_ADDR 0x15 + +#define ST_LSM6DSVX_REG_PAGE_RW_ADDR 0x17 +#define ST_LSM6DSVX_EMB_FUNC_LIR_MASK BIT(7) + +#define ST_LSM6DSVX_REG_EMB_FUNC_FIFO_EN_A_ADDR 0x44 +#define ST_LSM6DSVX_SFLP_GAME_FIFO_EN BIT(1) +#define ST_LSM6DSVX_SFLP_GRAVITY_FIFO_EN BIT(4) +#define ST_LSM6DSVX_SFLP_GBIAS_FIFO_EN_MASK BIT(5) +#define ST_LSM6DSVX_STEP_COUNTER_FIFO_EN_MASK BIT(6) + +#define ST_LSM6DSVX_REG_FSM_ENABLE_ADDR 0x46 + +#define ST_LSM6DSVX_REG_FSM_OUTS1_ADDR 0x4c + +#define ST_LSM6DSVX_REG_SFLP_ODR_ADDR 0x5e +#define ST_LSM6DSVX_SFLP_GAME_ODR_MASK GENMASK(5, 3) + +#define ST_LSM6DSVX_REG_FSM_ODR_ADDR 0x5f +#define ST_LSM6DSVX_FSM_ODR_MASK GENMASK(5, 3) + +#define ST_LSM6DSVX_REG_MLC_ODR_ADDR 0x60 +#define ST_LSM6DSVX_MLC_ODR_MASK GENMASK(6, 4) + +#define ST_LSM6DSVX_REG_STEP_COUNTER_L_ADDR 0x62 + +#define ST_LSM6DSVX_REG_EMB_FUNC_SRC_ADDR 0x64 +#define ST_LSM6DSVX_STEPCOUNTER_BIT_SET_MASK BIT(2) +#define ST_LSM6DSVX_STEP_OVERFLOW_MASK BIT(3) +#define ST_LSM6DSVX_STEP_COUNT_DELTA_IA_MASK BIT(4) +#define ST_LSM6DSVX_STEP_DETECTED_MASK BIT(5) +#define ST_LSM6DSVX_PEDO_RST_STEP_MASK BIT(7) + +#define ST_LSM6DSVX_REG_EMB_FUNC_INIT_A_ADDR 0x66 +#define ST_LSM6DSVX_SFLP_GAME_INIT_MASK BIT(1) + +#define ST_LSM6DSVX_REG_EMB_FUNC_INIT_B_ADDR 0x67 +#define ST_LSM6DSVX_FSM_INIT_MASK BIT(0) +#define ST_LSM6DSVX_MLC_INIT_MASK BIT(4) + +#define ST_LSM6DSVX_REG_MLC1_SRC_ADDR 0x70 + +/* SHUB */ +#define ST_LSM6DSVX_REG_SENSOR_HUB_1_ADDR 0x02 + +#define ST_LSM6DSVX_REG_MASTER_CONFIG_ADDR 0x14 +#define ST_LSM6DSVX_MASTER_ON_MASK BIT(2) +#define ST_LSM6DSVX_WRITE_ONCE_MASK BIT(6) + +#define ST_LSM6DSVX_REG_SLV0_ADDR 0x15 + +#define ST_LSM6DSVX_REG_SLV0_CONFIG_ADDR 0x17 +#define ST_LSM6DSVX_REG_SHUB_ODR_120HZ_VAL 0x04 +#define ST_LSM6DSVX_SHUB_ODR_MASK GENMASK(7, 5) +#define ST_LSM6DSVX_REG_BATCH_EXT_SENS_EN_MASK BIT(3) + +#define ST_LSM6DSVX_REG_SLV1_ADDR 0x18 +#define ST_LSM6DSVX_REG_SLV2_ADDR 0x1b +#define ST_LSM6DSVX_REG_SLV3_ADDR 0x1e + +#define ST_LSM6DSVX_REG_DATAWRITE_SLV0_ADDR 0x21 +#define ST_LSM6DSVX_REG_SLAVE_NUMOP_MASK GENMASK(2, 0) + +#define ST_LSM6DSVX_TS_DELTA_NS 21700ULL + +/* temperature in uC */ +#define ST_LSM6DSVX_TEMP_GAIN 256 +#define ST_LSM6DSVX_TEMP_OFFSET 6400 + +/* self test values */ +#define ST_LSM6DSVX_SELFTEST_ACCEL_MIN 410 +#define ST_LSM6DSVX_SELFTEST_ACCEL_MAX 13935 +#define ST_LSM6DSVX_SELFTEST_GYRO_MIN 2143 +#define ST_LSM6DSVX_SELFTEST_GYRO_MAX 10000 + +#define ST_LSM6DSVX_SELF_TEST_NORMAL_MODE_VAL 0 +#define ST_LSM6DSVX_SELF_TEST_POS_SIGN_VAL 1 +#define ST_LSM6DSVX_SELF_TEST_NEG_SIGN_VAL 2 + +#define ST_LSM6DSVX_DEFAULT_KTIME (200000000) +#define ST_LSM6DSVX_FAST_KTIME (5000000) + +#define ST_LSM6DSVX_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_LSM6DSVX_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_lsm6dsvx_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_event_spec st_lsm6dsvx_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_LSM6DSVX_EVENT_CHANNEL(ctype, etype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lsm6dsvx_##etype##_event, \ + .num_event_specs = 1, \ +} + +#define ST_LSM6DSVX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +struct st_lsm6dsvx_reg { + u8 addr; + u8 mask; +}; + +struct st_lsm6dsvx_odr { + u16 hz; + int uhz; + u8 val; + u8 batch_val; +}; + +struct st_lsm6dsvx_odr_table_entry { + u8 size; + struct st_lsm6dsvx_reg reg; + struct st_lsm6dsvx_odr odr_avl[ST_LSM6DSVX_ODR_LIST_SIZE]; +}; + +struct st_lsm6dsvx_fs { + struct st_lsm6dsvx_reg reg; + u32 gain; + u8 val; +}; + +#define ST_LSM6DSVX_FS_LIST_SIZE 6 +#define ST_LSM6DSVX_FS_ACC_LIST_SIZE 4 +#define ST_LSM6DSVX_FS_GYRO_LIST_SIZE 6 +struct st_lsm6dsvx_fs_table_entry { + u8 size; + struct st_lsm6dsvx_fs fs_avl[ST_LSM6DSVX_FS_LIST_SIZE]; +}; + +#define ST_LSM6DSVX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61000) +#define ST_LSM6DSVX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122000) +#define ST_LSM6DSVX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244000) +#define ST_LSM6DSVX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488000) + +#define ST_LSM6DSVX_GYRO_FS_125_GAIN IIO_DEGREE_TO_RAD(4375000) +#define ST_LSM6DSVX_GYRO_FS_250_GAIN IIO_DEGREE_TO_RAD(8750000) +#define ST_LSM6DSVX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500000) +#define ST_LSM6DSVX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000000) +#define ST_LSM6DSVX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000000) +#define ST_LSM6DSVX_GYRO_FS_4000_GAIN IIO_DEGREE_TO_RAD(140000000) + +struct st_lsm6dsvx_ext_dev_info { + const struct st_lsm6dsvx_ext_dev_settings *ext_dev_settings; + u8 ext_dev_i2c_addr; +}; + +/** + * enum st_lsm6dsvx_hw_id - list of HW device id supported by the + * lsm6dsvx driver + */ +enum st_lsm6dsvx_hw_id { + ST_LSM6DSV_ID, + ST_LSM6DSVX_ID, + ST_LSM6DSVX_MAX_ID, +}; + +enum st_lsm6dsvx_fsm_mlc_enable_id { + ST_LSM6DSVX_MLC_FSM_DISABLED = 0, + ST_LSM6DSVX_MLC_ENABLED = BIT(0), + ST_LSM6DSVX_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_lsm6dsvx_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_lsm6dsvx_fsm_mlc_enable_id status; +}; + +/** + * struct st_lsm6dsvx_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. + */ +struct st_lsm6dsvx_settings { + struct { + enum st_lsm6dsvx_hw_id hw_id; + const char *name; + } id; + + bool st_qvar_probe; + bool st_mlc_probe; + bool st_fsm_probe; + bool st_sflp_probe; +}; + +enum st_lsm6dsvx_sensor_id { + ST_LSM6DSVX_ID_GYRO, + ST_LSM6DSVX_ID_ACC, + ST_LSM6DSVX_ID_TEMP, + ST_LSM6DSVX_ID_6X_GAME, + ST_LSM6DSVX_ID_QVAR, + ST_LSM6DSVX_ID_EXT0, + ST_LSM6DSVX_ID_EXT1, + ST_LSM6DSVX_ID_MLC, + ST_LSM6DSVX_ID_MLC_0, + ST_LSM6DSVX_ID_MLC_1, + ST_LSM6DSVX_ID_MLC_2, + ST_LSM6DSVX_ID_MLC_3, + ST_LSM6DSVX_ID_FSM_0, + ST_LSM6DSVX_ID_FSM_1, + ST_LSM6DSVX_ID_FSM_2, + ST_LSM6DSVX_ID_FSM_3, + ST_LSM6DSVX_ID_FSM_4, + ST_LSM6DSVX_ID_FSM_5, + ST_LSM6DSVX_ID_FSM_6, + ST_LSM6DSVX_ID_FSM_7, + ST_LSM6DSVX_ID_STEP_COUNTER, + ST_LSM6DSVX_ID_STEP_DETECTOR, + ST_LSM6DSVX_ID_SIGN_MOTION, + ST_LSM6DSVX_ID_TILT, + ST_LSM6DSVX_ID_TAP, + ST_LSM6DSVX_ID_DTAP, + ST_LSM6DSVX_ID_FF, + ST_LSM6DSVX_ID_SLPCHG, + ST_LSM6DSVX_ID_WK, + ST_LSM6DSVX_ID_6D, + ST_LSM6DSVX_ID_MAX, +}; + +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_main_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_GYRO, + [1] = ST_LSM6DSVX_ID_ACC, + [2] = ST_LSM6DSVX_ID_6X_GAME, + [3] = ST_LSM6DSVX_ID_TEMP, +}; + +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_gyro_dep_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_GYRO, + [1] = ST_LSM6DSVX_ID_6X_GAME, + [2] = ST_LSM6DSVX_ID_EXT0, + [3] = ST_LSM6DSVX_ID_EXT1, + [4] = ST_LSM6DSVX_ID_MLC, + [5] = ST_LSM6DSVX_ID_MLC_0, + [6] = ST_LSM6DSVX_ID_MLC_1, + [7] = ST_LSM6DSVX_ID_MLC_2, + [8] = ST_LSM6DSVX_ID_MLC_3, + [9] = ST_LSM6DSVX_ID_FSM_0, + [10] = ST_LSM6DSVX_ID_FSM_1, + [11] = ST_LSM6DSVX_ID_FSM_2, + [12] = ST_LSM6DSVX_ID_FSM_3, + [13] = ST_LSM6DSVX_ID_FSM_4, + [14] = ST_LSM6DSVX_ID_FSM_5, + [15] = ST_LSM6DSVX_ID_FSM_6, + [16] = ST_LSM6DSVX_ID_FSM_7, +}; + +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_acc_dep_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_ACC, + [1] = ST_LSM6DSVX_ID_TEMP, + [2] = ST_LSM6DSVX_ID_6X_GAME, + [3] = ST_LSM6DSVX_ID_QVAR, + [4] = ST_LSM6DSVX_ID_EXT0, + [5] = ST_LSM6DSVX_ID_EXT1, + [6] = ST_LSM6DSVX_ID_MLC, + [7] = ST_LSM6DSVX_ID_MLC_0, + [8] = ST_LSM6DSVX_ID_MLC_1, + [9] = ST_LSM6DSVX_ID_MLC_2, + [10] = ST_LSM6DSVX_ID_MLC_3, + [11] = ST_LSM6DSVX_ID_FSM_0, + [12] = ST_LSM6DSVX_ID_FSM_1, + [13] = ST_LSM6DSVX_ID_FSM_2, + [14] = ST_LSM6DSVX_ID_FSM_3, + [15] = ST_LSM6DSVX_ID_FSM_4, + [16] = ST_LSM6DSVX_ID_FSM_5, + [17] = ST_LSM6DSVX_ID_FSM_6, + [18] = ST_LSM6DSVX_ID_FSM_7, + [19] = ST_LSM6DSVX_ID_STEP_COUNTER, + [20] = ST_LSM6DSVX_ID_STEP_DETECTOR, + [21] = ST_LSM6DSVX_ID_SIGN_MOTION, + [22] = ST_LSM6DSVX_ID_TILT, + [23] = ST_LSM6DSVX_ID_TAP, + [24] = ST_LSM6DSVX_ID_DTAP, + [25] = ST_LSM6DSVX_ID_FF, + [26] = ST_LSM6DSVX_ID_SLPCHG, + [27] = ST_LSM6DSVX_ID_WK, + [28] = ST_LSM6DSVX_ID_6D, +}; + +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_buffered_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_GYRO, + [1] = ST_LSM6DSVX_ID_ACC, + [2] = ST_LSM6DSVX_ID_TEMP, + [3] = ST_LSM6DSVX_ID_6X_GAME, + [4] = ST_LSM6DSVX_ID_QVAR, + [5] = ST_LSM6DSVX_ID_EXT0, + [6] = ST_LSM6DSVX_ID_EXT1, + [7] = ST_LSM6DSVX_ID_STEP_COUNTER, +}; + +/** + * The mlc only sensor list used by mlc loader + */ +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_mlc_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_MLC_0, + [1] = ST_LSM6DSVX_ID_MLC_1, + [2] = ST_LSM6DSVX_ID_MLC_2, + [3] = ST_LSM6DSVX_ID_MLC_3, +}; + +/** + * The fsm only sensor list used by mlc loader + */ +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_fsm_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_FSM_0, + [1] = ST_LSM6DSVX_ID_FSM_1, + [2] = ST_LSM6DSVX_ID_FSM_2, + [3] = ST_LSM6DSVX_ID_FSM_3, + [4] = ST_LSM6DSVX_ID_FSM_4, + [5] = ST_LSM6DSVX_ID_FSM_5, + [6] = ST_LSM6DSVX_ID_FSM_6, + [7] = ST_LSM6DSVX_ID_FSM_7, +}; + +/** + * The low power embedded function only sensor list + */ +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_embfunc_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_STEP_COUNTER, + [1] = ST_LSM6DSVX_ID_STEP_DETECTOR, + [2] = ST_LSM6DSVX_ID_SIGN_MOTION, + [3] = ST_LSM6DSVX_ID_TILT, +}; + +/** + * The low power event only sensor list + */ +static const enum st_lsm6dsvx_sensor_id st_lsm6dsvx_event_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_TAP, + [1] = ST_LSM6DSVX_ID_DTAP, + [2] = ST_LSM6DSVX_ID_FF, + [3] = ST_LSM6DSVX_ID_SLPCHG, + [4] = ST_LSM6DSVX_ID_WK, + [5] = ST_LSM6DSVX_ID_6D, +}; + +/** + * The low power event triggered only sensor list + */ +static const enum st_lsm6dsvx_sensor_id +st_lsm6dsvx_event_trigger_sensor_list[] = { + [0] = ST_LSM6DSVX_ID_WK, + [1] = ST_LSM6DSVX_ID_6D, +}; + +#define ST_LSM6DSVX_ID_ALL_FSM_MLC (BIT(ST_LSM6DSVX_ID_MLC_0) | \ + BIT(ST_LSM6DSVX_ID_MLC_1) | \ + BIT(ST_LSM6DSVX_ID_MLC_2) | \ + BIT(ST_LSM6DSVX_ID_MLC_3) | \ + BIT(ST_LSM6DSVX_ID_FSM_0) | \ + BIT(ST_LSM6DSVX_ID_FSM_1) | \ + BIT(ST_LSM6DSVX_ID_FSM_2) | \ + BIT(ST_LSM6DSVX_ID_FSM_3) | \ + BIT(ST_LSM6DSVX_ID_FSM_4) | \ + BIT(ST_LSM6DSVX_ID_FSM_5) | \ + BIT(ST_LSM6DSVX_ID_FSM_6) | \ + BIT(ST_LSM6DSVX_ID_FSM_7)) +enum st_lsm6dsvx_fifo_mode { + ST_LSM6DSVX_FIFO_BYPASS = 0x0, + ST_LSM6DSVX_FIFO_CONT = 0x6, +}; + +enum { + ST_LSM6DSVX_HW_FLUSH, + ST_LSM6DSVX_HW_OPERATIONAL, +}; + +/* sensor devices that can wake-up the target */ +#define ST_LSM6DSVX_WAKE_UP_SENSORS (BIT(ST_LSM6DSVX_ID_GYRO) | \ + BIT(ST_LSM6DSVX_ID_ACC) | \ + ST_LSM6DSVX_ID_ALL_FSM_MLC) + +/** + * struct st_lsm6dsvx_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dsvx_hw. + * @ext_dev_info: Sensor hub i2c slave settings. + * @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. + * @hr_timer: hr timer for qvar. + * @iio_work: iio work for qvar. + * @oldktime: hr timeout for qvar. + * @timestamp: qvar timestamp (when in polling mode). + * @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_lsm6dsvx_sensor { + char name[32]; + enum st_lsm6dsvx_sensor_id id; + struct st_lsm6dsvx_hw *hw; + + struct st_lsm6dsvx_ext_dev_info ext_dev_info; + + struct iio_trigger *trig; + + int odr; + int uodr; + + union { + struct { + /* data sensors */ + u32 gain; + u32 offset; + +#ifndef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + struct hrtimer hr_timer; + struct work_struct iio_work; + ktime_t oldktime; + s64 timestamp; +#endif /* !CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + 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_lsm6dsvx_reg batch_reg; + }; + + struct { + /* mlc or fsm sensor */ + uint8_t status_reg; + uint8_t outreg_addr; + enum st_lsm6dsvx_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_lsm6dsvx_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. + */ +struct st_lsm6dsvx_hw { + struct device *dev; + int irq; + struct regmap *regmap; + struct mutex lock; + struct mutex fifo_lock; + struct mutex page_lock; + + enum st_lsm6dsvx_fifo_mode fifo_mode; + unsigned long state; + u32 enable_mask; + s64 hw_timestamp_global; + +#if defined(CONFIG_IIO_ST_LSM6DSVX_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_LSM6DSVX_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_lsm6dsvx_mlc_config_t *mlc_config; + bool preload_mlc; + struct workqueue_struct *qvar_workqueue; + struct iio_dev *iio_devs[ST_LSM6DSVX_ID_MAX]; + + const struct st_lsm6dsvx_settings *settings; + const struct st_lsm6dsvx_fs_table_entry *fs_table; + const struct st_lsm6dsvx_odr_table_entry *odr_table; +}; + +/** + * struct st_lsm6dsvx_ff_th - Free Fall threshold table + * @mg: Threshold in mg. + * @val: Register value. + */ +struct st_lsm6dsvx_ff_th { + u32 mg; + u8 val; +}; + +/** + * struct st_lsm6dsvx_6D_th - 6D threshold table + * @deg: Threshold in degrees. + * @val: Register value. + */ +struct st_lsm6dsvx_6D_th { + u8 deg; + u8 val; +}; + +extern const struct dev_pm_ops st_lsm6dsvx_pm_ops; + +static inline int +__st_lsm6dsvx_write_with_mask(struct st_lsm6dsvx_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_LSM6DSVX_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int st_lsm6dsvx_write_with_mask(struct st_lsm6dsvx_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dsvx_write_with_mask(hw, addr, mask, data); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsvx_read_locked(struct st_lsm6dsvx_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_lsm6dsvx_write_locked(struct st_lsm6dsvx_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_lsm6dsvx_read_with_mask(struct st_lsm6dsvx_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_lsm6dsvx_set_page_access(struct st_lsm6dsvx_hw *hw, + unsigned int mask, + unsigned int data) +{ + return regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_FUNC_CFG_ACCESS_ADDR, + mask, + ST_LSM6DSVX_SHIFT_VAL(data, mask)); +} + +static inline bool st_lsm6dsvx_run_mlc_task(struct st_lsm6dsvx_hw *hw) +{ + return hw->settings->st_mlc_probe || hw->settings->st_fsm_probe; +} + +static inline bool +st_lsm6dsvx_is_fifo_enabled(struct st_lsm6dsvx_hw *hw) +{ + return hw->enable_mask & (BIT(ST_LSM6DSVX_ID_GYRO) | + BIT(ST_LSM6DSVX_ID_ACC) | + BIT(ST_LSM6DSVX_ID_EXT0) | + BIT(ST_LSM6DSVX_ID_EXT1) | + BIT(ST_LSM6DSVX_ID_QVAR)); +} + +int st_lsm6dsvx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap); +int st_lsm6dsvx_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable); +int st_lsm6dsvx_sflp_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable); +int st_lsm6dsvx_shub_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable); +int st_lsm6dsvx_buffers_setup(struct st_lsm6dsvx_hw *hw); +ssize_t st_lsm6dsvx_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsvx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsvx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lsm6dsvx_set_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lsm6dsvx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf); +int st_lsm6dsvx_suspend_fifo(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_set_fifo_mode(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_fifo_mode fifo_mode); +int +__st_lsm6dsvx_set_sensor_batching_odr(struct st_lsm6dsvx_sensor *sensor, + bool enable); +int st_lsm6dsvx_fsm_init(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_fsm_get_orientation(struct st_lsm6dsvx_hw *hw, + u8 *data); +int st_lsm6dsvx_update_batching(struct iio_dev *iio_dev, bool enable); +int st_lsm6dsvx_get_batch_val(struct st_lsm6dsvx_sensor *sensor, + int odr, int uodr, u8 *val); +int st_lsm6dsvx_remove(struct device *dev); + +int st_lsm6dsvx_shub_probe(struct st_lsm6dsvx_hw *hw); + +int st_lsm6dsvx_qvar_probe(struct st_lsm6dsvx_hw *hw); +int +st_lsm6dsvx_qvar_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable); +int st_lsm6dsvx_qvar_remove(struct device *dev); + +#if defined(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) +int st_lsm6dsvx_hwtimesync_init(struct st_lsm6dsvx_hw *hw); +#else /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ +static inline int +st_lsm6dsvx_hwtimesync_init(struct st_lsm6dsvx_hw *hw) +{ + return 0; +} +#endif /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ + +int st_lsm6dsvx_mlc_probe(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_mlc_remove(struct device *dev); +int st_lsm6dsvx_mlc_check_status(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_mlc_init_preload(struct st_lsm6dsvx_hw *hw); + +int st_lsm6dsvx_probe_event(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_event_handler(struct st_lsm6dsvx_hw *hw); + +int st_lsm6dsvx_probe_embfunc(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_embfunc_handler_thread(struct st_lsm6dsvx_hw *hw); +int st_lsm6dsvx_step_counter_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable); +#endif /* ST_LSM6DSVX_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_buffer.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_buffer.c new file mode 100644 index 000000000000..b112a830ad4c --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_buffer.c @@ -0,0 +1,786 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx FIFO buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +#define ST_LSM6DSVX_SAMPLE_DISCHARD 0x7ffd +#define ST_LSM6DSVX_QVAR_FILTER_X 0x03AA +#define ST_LSM6DSVX_QVAR_SAMPLE_SIZE 2 + +enum { + ST_LSM6DSVX_GYRO_TAG = 0x01, + ST_LSM6DSVX_ACC_TAG = 0x02, + ST_LSM6DSVX_TEMP_TAG = 0x03, + ST_LSM6DSVX_TS_TAG = 0x04, + ST_LSM6DSVX_EXT0_TAG = 0x0f, + ST_LSM6DSVX_EXT1_TAG = 0x10, + ST_LSM6DSVX_STEPC_TAG = 0x12, + ST_LSM6DSVX_GAMEROT_TAG = 0x13, +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + ST_LSM6DSVX_QVAR_TAG = 0x1B, +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ +}; + +#define ST_LSM6DSVX_EWMA_LEVEL 120 +#define ST_LSM6DSVX_EWMA_DIV 128 + +#define ST_LSM6DSVX_TIMESTAMP_RESET_VALUE 0xaa + +static inline s64 st_lsm6dsvx_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LSM6DSVX_EWMA_DIV - weight) * diff, + ST_LSM6DSVX_EWMA_DIV); + + return old + incr; +} + +static inline int st_lsm6dsvx_reset_hwts(struct st_lsm6dsvx_hw *hw) +{ + u8 data = ST_LSM6DSVX_TIMESTAMP_RESET_VALUE; + int ret; + + ret = st_lsm6dsvx_write_locked(hw, ST_LSM6DSVX_REG_TIMESTAMP2_ADDR, + data); + if (ret < 0) + return ret; + +#if defined(CONFIG_IIO_ST_LSM6DSVX_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_LSM6DSVX_FAST_KTIME); +#else /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ + hw->hw_timestamp_global = (hw->hw_timestamp_global + (1LL << 32)) & + GENMASK_ULL(63, 32); +#endif /* CONFIG_IIO_ST_LSM6DSVX_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_lsm6dsvx_set_fifo_mode(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_fifo_mode fifo_mode) +{ + int err; + + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSVX_FIFO_MODE_MASK, + fifo_mode); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +int +__st_lsm6dsvx_set_sensor_batching_odr(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 data = 0; + int err; + + if (enable) { + err = st_lsm6dsvx_get_batch_val(sensor, sensor->odr, + sensor->uodr, &data); + if (err < 0) + return err; + } + + if (sensor->id == ST_LSM6DSVX_ID_6X_GAME) { + st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 1); + err = __st_lsm6dsvx_write_with_mask(hw, sensor->batch_reg.addr, + sensor->batch_reg.mask, + data); + st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + } else { + err = __st_lsm6dsvx_write_with_mask(hw, sensor->batch_reg.addr, + sensor->batch_reg.mask, + data); + } + + return err; +} + +static inline int +st_lsm6dsvx_set_sensor_batching_odr(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + err = __st_lsm6dsvx_set_sensor_batching_odr(sensor, enable); + mutex_unlock(&hw->page_lock); + + return err; +} + +static int +st_lsm6dsvx_update_watermark(struct st_lsm6dsvx_sensor *sensor, u16 watermark) +{ + u16 fifo_watermark = ST_LSM6DSVX_MAX_FIFO_DEPTH; + struct st_lsm6dsvx_hw *hw = sensor->hw; + struct st_lsm6dsvx_sensor *cur_sensor; + u16 cur_watermark = 0; + __le16 wdata; + int data = 0; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_buffered_sensor_list); i++) { + enum st_lsm6dsvx_sensor_id id = st_lsm6dsvx_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_LSM6DSVX_REG_FIFO_CTRL1_ADDR + 1, &data); + if (err < 0) + goto out; + + fifo_watermark = ((data << 8) & ~ST_LSM6DSVX_WTM_MASK) | + (fifo_watermark & ST_LSM6DSVX_WTM_MASK); + wdata = cpu_to_le16(fifo_watermark); + err = regmap_bulk_write(hw->regmap, ST_LSM6DSVX_REG_FIFO_CTRL1_ADDR, + &wdata, sizeof(wdata)); +out: + mutex_unlock(&hw->page_lock); + + return err < 0 ? err : 0; +} + +static inline void st_lsm6dsvx_sync_hw_ts(struct st_lsm6dsvx_hw *hw, s64 ts) +{ + s64 delta = ts - hw->hw_ts; + + hw->ts_offset = st_lsm6dsvx_ewma(hw->ts_offset, delta, + ST_LSM6DSVX_EWMA_LEVEL); +} + +static struct iio_dev * +st_lsm6dsvx_get_iiodev_from_tag(struct st_lsm6dsvx_hw *hw, u8 tag) +{ + struct iio_dev *iio_dev; + + switch (tag) { + case ST_LSM6DSVX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_GYRO]; + break; + case ST_LSM6DSVX_ACC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_ACC]; + break; + case ST_LSM6DSVX_EXT0_TAG: + if (hw->enable_mask & BIT(ST_LSM6DSVX_ID_EXT0)) + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_EXT0]; + else + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_EXT1]; + break; + case ST_LSM6DSVX_EXT1_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_EXT1]; + break; + case ST_LSM6DSVX_TEMP_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_TEMP]; + break; + case ST_LSM6DSVX_STEPC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_STEP_COUNTER]; + break; + +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + case ST_LSM6DSVX_QVAR_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_QVAR]; + break; +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + case ST_LSM6DSVX_GAMEROT_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_6X_GAME]; + break; + default: + iio_dev = NULL; + break; + } + + return iio_dev; +} + +static int st_lsm6dsvx_read_fifo(struct st_lsm6dsvx_hw *hw) +{ + u8 iio_buf[ALIGN(ST_LSM6DSVX_FIFO_SAMPLE_SIZE, sizeof(s64)) + + sizeof(s64) + sizeof(s64)]; + u8 buf[6 * ST_LSM6DSVX_FIFO_SAMPLE_SIZE], tag, *ptr; + int i, err, word_len, fifo_len, read_len; + struct st_lsm6dsvx_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_lsm6dsvx_read_locked(hw, ST_LSM6DSVX_REG_FIFO_STATUS1_ADDR, + &fifo_status, sizeof(fifo_status)); + if (err < 0) + return err; + + fifo_depth = le16_to_cpu(fifo_status) & ST_LSM6DSVX_FIFO_DIFF_MASK; + if (!fifo_depth) + return 0; + + fifo_len = fifo_depth * ST_LSM6DSVX_FIFO_SAMPLE_SIZE; + read_len = 0; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buf)); + err = st_lsm6dsvx_read_locked(hw, + ST_LSM6DSVX_REG_FIFO_DATA_OUT_TAG_ADDR, + buf, word_len); + if (err < 0) + return err; + + for (i = 0; i < word_len; + i += ST_LSM6DSVX_FIFO_SAMPLE_SIZE) { + ptr = &buf[i + ST_LSM6DSVX_TAG_SIZE]; + tag = buf[i] >> 3; + + if (tag == ST_LSM6DSVX_TS_TAG) { + val = get_unaligned_le32(ptr); + if (hw->val_ts_old > val) + hw->hw_ts_high++; + +#if defined(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSVX_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_LSM6DSVX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSVX_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_lsm6dsvx_ewma(hw->ts_offset, + ts_irq - hw->hw_ts, + ST_LSM6DSVX_EWMA_LEVEL); + + if (!test_bit(ST_LSM6DSVX_HW_FLUSH, &hw->state)) + /* sync ap timestamp and sensor one */ + st_lsm6dsvx_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_lsm6dsvx_get_iiodev_from_tag(hw, + tag); + if (!iio_dev) + continue; + +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + /* qvar data is on X axis of the filter */ + if (tag == ST_LSM6DSVX_QVAR_TAG) { + u16 id_f = (u16)le16_to_cpu(get_unaligned_le16(ptr + 2)); + + if (id_f != ST_LSM6DSVX_QVAR_FILTER_X) + continue; + + memcpy(iio_buf, ptr, + ST_LSM6DSVX_QVAR_SAMPLE_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, + iio_buf, + iio_get_time_ns(hw->iio_devs[0])); + } else { +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + drdymask = (s16)le16_to_cpu(get_unaligned_le16(ptr)); + if (unlikely(drdymask >= ST_LSM6DSVX_SAMPLE_DISCHARD)) + continue; + + sensor = iio_priv(iio_dev); + + /* + * hw ts in not queued in FIFO if only step + * counter enabled + */ + if (sensor->id == ST_LSM6DSVX_ID_STEP_COUNTER) { + val = get_unaligned_le32(ptr + 2); + hw->tsample = val * hw->ts_delta_ns; + } + memcpy(iio_buf, ptr, + ST_LSM6DSVX_SAMPLE_SIZE); + +#if defined(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) + spin_lock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ + + hw_timestamp_push = cpu_to_le64(hw->hw_timestamp_global); + +#if defined(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) + spin_unlock_irq(&hw->hwtimestamp_lock); +#endif /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ + + memcpy(&iio_buf[ALIGN(ST_LSM6DSVX_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; + } +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + } +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + } + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lsm6dsvx_get_max_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->max_watermark); +} + +ssize_t st_lsm6dsvx_get_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", sensor->watermark); +} + +ssize_t st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_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_LSM6DSVX_HW_FLUSH, &hw->state); + count = st_lsm6dsvx_read_fifo(hw); + sensor->dec_counter = 0; + fifo_ts = sensor->last_fifo_timestamp; + mutex_unlock(&hw->fifo_lock); + + type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(iio_dev, event, fifo_ts); + + return size; +} + +int st_lsm6dsvx_suspend_fifo(struct st_lsm6dsvx_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + + st_lsm6dsvx_read_fifo(hw); + err = st_lsm6dsvx_set_fifo_mode(hw, ST_LSM6DSVX_FIFO_BYPASS); + + mutex_unlock(&hw->fifo_lock); + + return err; +} + +int st_lsm6dsvx_update_batching(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + err = st_lsm6dsvx_set_sensor_batching_odr(sensor, enable); + enable_irq(hw->irq); + + return err; +} + +int st_lsm6dsvx_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err; + + disable_irq(hw->irq); + +#if defined(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) + hrtimer_cancel(&hw->timesync_timer); + cancel_work_sync(&hw->timesync_work); +#endif /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ + + switch (sensor->id) { + +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + case ST_LSM6DSVX_ID_QVAR: + err = st_lsm6dsvx_qvar_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + break; +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + case ST_LSM6DSVX_ID_EXT0: + case ST_LSM6DSVX_ID_EXT1: + err = st_lsm6dsvx_shub_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSVX_ID_6X_GAME: + err = st_lsm6dsvx_sflp_set_enable(sensor, enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSVX_ID_STEP_COUNTER: + err = st_lsm6dsvx_step_counter_set_enable(sensor, + enable); + if (err < 0) + goto out; + break; + case ST_LSM6DSVX_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_LSM6DSVX_ID_ACC) | + BIT(ST_LSM6DSVX_ID_GYRO)))) { + struct st_lsm6dsvx_sensor *acc_sensor; + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSVX_ID_ACC]); + if (enable) { + err = st_lsm6dsvx_get_batch_val(acc_sensor, + sensor->odr, + sensor->uodr, + &data); + if (err < 0) + goto out; + } + + /* batch main sensor */ + err = st_lsm6dsvx_write_with_mask(hw, + acc_sensor->batch_reg.addr, + acc_sensor->batch_reg.mask, + data); + if (err < 0) + goto out; + } + + if (enable) { + err = st_lsm6dsvx_get_batch_val(sensor, sensor->odr, + sensor->uodr, &data); + if (err < 0) + goto out; + } + + /* batch temperature sensor */ + err = st_lsm6dsvx_write_with_mask(hw, sensor->batch_reg.addr, + sensor->batch_reg.mask, + data); + if (err < 0) + goto out; + + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + } + break; + default: + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + + err = st_lsm6dsvx_set_sensor_batching_odr(sensor, enable); + if (err < 0) + goto out; + + break; + } + + err = st_lsm6dsvx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (enable && hw->fifo_mode == ST_LSM6DSVX_FIFO_BYPASS) { + st_lsm6dsvx_reset_hwts(hw); + err = st_lsm6dsvx_set_fifo_mode(hw, ST_LSM6DSVX_FIFO_CONT); + } else if (!hw->enable_mask) { + err = st_lsm6dsvx_set_fifo_mode(hw, ST_LSM6DSVX_FIFO_BYPASS); + } + +#if defined(CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP) + if (hw->fifo_mode != ST_LSM6DSVX_FIFO_BYPASS) { + hrtimer_start(&hw->timesync_timer, + ktime_set(0, 0), + HRTIMER_MODE_REL); + } +#endif /* CONFIG_IIO_ST_LSM6DSVX_ASYNC_HW_TIMESTAMP */ + +out: + enable_irq(hw->irq); + + return err; +} + +static irqreturn_t st_lsm6dsvx_handler_irq(int irq, void *private) +{ + struct st_lsm6dsvx_hw *hw = (struct st_lsm6dsvx_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_lsm6dsvx_handler_thread(int irq, void *private) +{ + struct st_lsm6dsvx_hw *hw = (struct st_lsm6dsvx_hw *)private; + + if (st_lsm6dsvx_run_mlc_task(hw)) + st_lsm6dsvx_mlc_check_status(hw); + + mutex_lock(&hw->fifo_lock); + st_lsm6dsvx_read_fifo(hw); + clear_bit(ST_LSM6DSVX_HW_FLUSH, &hw->state); + mutex_unlock(&hw->fifo_lock); + + st_lsm6dsvx_event_handler(hw); + st_lsm6dsvx_embfunc_handler_thread(hw); + + return IRQ_HANDLED; +} + +static int st_lsm6dsvx_fifo_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsvx_update_fifo(iio_dev, true); +} + +static int st_lsm6dsvx_fifo_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsvx_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsvx_fifo_ops = { + .preenable = st_lsm6dsvx_fifo_preenable, + .postdisable = st_lsm6dsvx_fifo_postdisable, +}; + +static int st_lsm6dsvx_fifo_init(struct st_lsm6dsvx_hw *hw) +{ + return st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_FIFO_CTRL4_ADDR, + ST_LSM6DSVX_DEC_TS_BATCH_MASK, 1); +} + +static const struct iio_trigger_ops st_lsm6dsvx_trigger_ops = { + NULL +}; + +int st_lsm6dsvx_buffers_setup(struct st_lsm6dsvx_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_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_IF_CFG_ADDR, + ST_LSM6DSVX_H_LACTIVE_MASK, + irq_active_low); + if (err < 0) + return err; + + if (np && of_property_read_bool(np, "drive-open-drain")) { + err = st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_IF_CFG_ADDR, + ST_LSM6DSVX_PP_OD_MASK, 1); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lsm6dsvx_handler_irq, + st_lsm6dsvx_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_lsm6dsvx_buffered_sensor_list); i++) { + enum st_lsm6dsvx_sensor_id id = + st_lsm6dsvx_buffered_sensor_list[i]; + + if (!hw->iio_devs[id]) + continue; + +#if KERNEL_VERSION(5, 13, 0) <= LINUX_VERSION_CODE + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[id], + INDIO_BUFFER_SOFTWARE, + &st_lsm6dsvx_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_lsm6dsvx_fifo_ops; + } +#endif /* LINUX_VERSION_CODE */ + } + + err = st_lsm6dsvx_hwtimesync_init(hw); + if (err) + return err; + + return st_lsm6dsvx_fifo_init(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_core.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_core.c new file mode 100644 index 000000000000..629376bfd9bb --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_core.c @@ -0,0 +1,1795 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx imu sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsvx.h" + +/** + * List of supported self test mode + */ +static struct st_lsm6dsvx_selftest_table_t { + char *smode; + u8 value; + u8 mask; +} st_lsm6dsvx_selftest_table[] = { + [0] = { + .smode = "disabled", + .value = ST_LSM6DSVX_SELF_TEST_NORMAL_MODE_VAL, + }, + [1] = { + .smode = "positive-sign", + .value = ST_LSM6DSVX_SELF_TEST_POS_SIGN_VAL, + }, + [2] = { + .smode = "negative-sign", + .value = ST_LSM6DSVX_SELF_TEST_NEG_SIGN_VAL, + }, +}; + +/** + * List of supported device settings + * + * The following table list all device supported by st_lsm6dsvx driver. + */ +static const struct st_lsm6dsvx_settings st_lsm6dsvx_sensor_settings[] = { + { + .id = { + .hw_id = ST_LSM6DSV_ID, + .name = ST_LSM6DSV_DEV_NAME, + }, + .st_fsm_probe = true, + }, + { + .id = { + .hw_id = ST_LSM6DSVX_ID, + .name = ST_LSM6DSV16X_DEV_NAME, + }, + .st_qvar_probe = true, + .st_mlc_probe = true, + .st_fsm_probe = true, + .st_sflp_probe = true, + }, +}; + +static const struct st_lsm6dsvx_odr_table_entry +st_lsm6dsvx_odr_table[] = { + [ST_LSM6DSVX_ID_ACC] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL1_ADDR, + .mask = GENMASK(6, 0), + }, + /* odr val batch */ + .odr_avl[1] = { 7, 0, 0x03, 0x02 }, + .odr_avl[2] = { 15, 0, 0x03, 0x03 }, + .odr_avl[3] = { 30, 0, 0x04, 0x04 }, + .odr_avl[4] = { 60, 0, 0x05, 0x05 }, + .odr_avl[5] = { 120, 0, 0x06, 0x06 }, + .odr_avl[6] = { 240, 0, 0x07, 0x07 }, + .odr_avl[7] = { 480, 0, 0x08, 0x08 }, + .odr_avl[8] = { 960, 0, 0x09, 0x09 }, + }, + [ST_LSM6DSVX_ID_GYRO] = { + .size = 8, + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL2_ADDR, + .mask = GENMASK(6, 0), + }, + /* G LP MODE 7 Hz batch 7 Hz */ + .odr_avl[1] = { 7, 0, 0x52, 0x02 }, + .odr_avl[2] = { 15, 0, 0x03, 0x03 }, + .odr_avl[3] = { 30, 0, 0x04, 0x04 }, + .odr_avl[4] = { 60, 0, 0x05, 0x05 }, + .odr_avl[5] = { 120, 0, 0x06, 0x06 }, + .odr_avl[6] = { 240, 0, 0x07, 0x07 }, + .odr_avl[7] = { 480, 0, 0x08, 0x08 }, + .odr_avl[8] = { 960, 0, 0x09, 0x09 }, + }, + [ST_LSM6DSVX_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_LSM6DSVX_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 st_lsm6dsvx_fs_table_entry st_lsm6dsvx_fs_table[] = { + [ST_LSM6DSVX_ID_ACC] = { + .size = 4, + .fs_avl[0] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL8_ADDR, + .mask = GENMASK(1, 0), + }, + .gain = ST_LSM6DSVX_ACC_FS_2G_GAIN, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL8_ADDR, + .mask = GENMASK(1, 0), + }, + .gain = ST_LSM6DSVX_ACC_FS_4G_GAIN, + .val = 0x1, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL8_ADDR, + .mask = GENMASK(1, 0), + }, + .gain = ST_LSM6DSVX_ACC_FS_8G_GAIN, + .val = 0x2, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL8_ADDR, + .mask = GENMASK(1, 0), + }, + .gain = ST_LSM6DSVX_ACC_FS_16G_GAIN, + .val = 0x3, + }, + }, + [ST_LSM6DSVX_ID_GYRO] = { + .size = 6, + .fs_avl[0] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_LSM6DSVX_GYRO_FS_125_GAIN, + .val = 0x0, + }, + .fs_avl[1] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_LSM6DSVX_GYRO_FS_250_GAIN, + .val = 0x1, + }, + .fs_avl[2] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_LSM6DSVX_GYRO_FS_500_GAIN, + .val = 0x2, + }, + .fs_avl[3] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_LSM6DSVX_GYRO_FS_1000_GAIN, + .val = 0x3, + }, + .fs_avl[4] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_LSM6DSVX_GYRO_FS_2000_GAIN, + .val = 0x4, + }, + .fs_avl[5] = { + .reg = { + .addr = ST_LSM6DSVX_REG_CTRL6_ADDR, + .mask = GENMASK(3, 0), + }, + .gain = ST_LSM6DSVX_GYRO_FS_4000_GAIN, + .val = 0x6, + }, + }, + [ST_LSM6DSVX_ID_TEMP] = { + .size = 1, + .fs_avl[0] = { + .reg = { 0 }, + .gain = (1000000 / ST_LSM6DSVX_TEMP_GAIN), + .val = 0x0 + }, + }, +}; + +static const struct iio_mount_matrix * +st_lsm6dsvx_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *ch) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const struct iio_chan_spec_ext_info st_lsm6dsvx_chan_spec_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, st_lsm6dsvx_get_mount_matrix), + { } +}; + +#define IIO_CHAN_HW_TIMESTAMP(si) { \ + .type = IIO_COUNT, \ + .address = ST_LSM6DSVX_REG_TIMESTAMP0_ADDR, \ + .scan_index = si, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 64, \ + .storagebits = 64, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_lsm6dsvx_acc_channels[] = { + ST_LSM6DSVX_DATA_CHANNEL(IIO_ACCEL, + ST_LSM6DSVX_REG_OUTX_L_A_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsvx_chan_spec_ext_info), + ST_LSM6DSVX_DATA_CHANNEL(IIO_ACCEL, + ST_LSM6DSVX_REG_OUTY_L_A_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsvx_chan_spec_ext_info), + ST_LSM6DSVX_DATA_CHANNEL(IIO_ACCEL, + ST_LSM6DSVX_REG_OUTZ_L_A_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsvx_chan_spec_ext_info), + ST_LSM6DSVX_EVENT_CHANNEL(IIO_ACCEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsvx_gyro_channels[] = { + ST_LSM6DSVX_DATA_CHANNEL(IIO_ANGL_VEL, + ST_LSM6DSVX_REG_OUTX_L_G_ADDR, + 1, IIO_MOD_X, 0, 16, 16, 's', + st_lsm6dsvx_chan_spec_ext_info), + ST_LSM6DSVX_DATA_CHANNEL(IIO_ANGL_VEL, + ST_LSM6DSVX_REG_OUTY_L_G_ADDR, + 1, IIO_MOD_Y, 1, 16, 16, 's', + st_lsm6dsvx_chan_spec_ext_info), + ST_LSM6DSVX_DATA_CHANNEL(IIO_ANGL_VEL, + ST_LSM6DSVX_REG_OUTZ_L_G_ADDR, + 1, IIO_MOD_Z, 2, 16, 16, 's', + st_lsm6dsvx_chan_spec_ext_info), + ST_LSM6DSVX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_HW_TIMESTAMP(3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static const struct iio_chan_spec st_lsm6dsvx_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LSM6DSVX_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_LSM6DSVX_EVENT_CHANNEL(IIO_TEMP, flush), + IIO_CHAN_HW_TIMESTAMP(1), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +static const struct iio_chan_spec st_lsm6dsvx_sflp_channels[] = { + ST_LSM6DSVX_SFLP_DATA_CHANNEL(IIO_ROT, 1, IIO_MOD_X, + 0, 16, 16, 'u'), + ST_LSM6DSVX_SFLP_DATA_CHANNEL(IIO_ROT, 1, IIO_MOD_Y, + 1, 16, 16, 'u'), + ST_LSM6DSVX_SFLP_DATA_CHANNEL(IIO_ROT, 1, IIO_MOD_Z, + 2, 16, 16, 'u'), + ST_LSM6DSVX_EVENT_CHANNEL(IIO_ANGL_VEL, flush), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static int st_lsm6dsvx_check_whoami(struct st_lsm6dsvx_hw *hw, int id) +{ + int data, err, i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_sensor_settings); i++) { + if (st_lsm6dsvx_sensor_settings[i].id.name && + st_lsm6dsvx_sensor_settings[i].id.hw_id == id) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsvx_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_LSM6DSVX_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + + return err; + } + + if (data != ST_LSM6DSVX_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + + return -ENODEV; + } + + hw->settings = &st_lsm6dsvx_sensor_settings[i]; + hw->fs_table = st_lsm6dsvx_fs_table; + hw->odr_table = st_lsm6dsvx_odr_table; + + return 0; +} + +static int st_lsm6dsvx_get_odr_calibration(struct st_lsm6dsvx_hw *hw) +{ + s64 odr_calib; + int data; + int err; + + err = regmap_read(hw->regmap, + ST_LSM6DSVX_REG_INTERNAL_FREQ_FINE, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %d register\n", + ST_LSM6DSVX_REG_INTERNAL_FREQ_FINE); + + return err; + } + + odr_calib = (data * 37500) / 1000; + hw->ts_delta_ns = ST_LSM6DSVX_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_lsm6dsvx_set_full_scale(struct st_lsm6dsvx_sensor *sensor, u32 gain) +{ + enum st_lsm6dsvx_sensor_id id = sensor->id; + int i, err; + u8 val; + + for (i = 0; i < st_lsm6dsvx_fs_table[id].size; i++) + if (st_lsm6dsvx_fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == st_lsm6dsvx_fs_table[id].size) + return -EINVAL; + + val = st_lsm6dsvx_fs_table[id].fs_avl[i].val; + err = st_lsm6dsvx_write_with_mask(sensor->hw, + st_lsm6dsvx_fs_table[id].fs_avl[i].reg.addr, + st_lsm6dsvx_fs_table[id].fs_avl[i].reg.mask, + val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dsvx_get_odr_val(enum st_lsm6dsvx_sensor_id id, + int odr, int uodr, int *podr, + int *puodr, u8 *val) +{ + int required_odr = ST_LSM6DSVX_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < st_lsm6dsvx_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSVX_ODR_EXPAND( + st_lsm6dsvx_odr_table[id].odr_avl[i].hz, + st_lsm6dsvx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= required_odr) + break; + } + + if (i == st_lsm6dsvx_odr_table[id].size) + return -EINVAL; + + *val = st_lsm6dsvx_odr_table[id].odr_avl[i].val; + + if (podr && puodr) { + *podr = st_lsm6dsvx_odr_table[id].odr_avl[i].hz; + *puodr = st_lsm6dsvx_odr_table[id].odr_avl[i].uhz; + } + + return 0; +} + +int st_lsm6dsvx_get_batch_val(struct st_lsm6dsvx_sensor *sensor, + int odr, int uodr, u8 *val) +{ + int required_odr = ST_LSM6DSVX_ODR_EXPAND(odr, uodr); + enum st_lsm6dsvx_sensor_id id = sensor->id; + int sensor_odr; + int i; + + for (i = 0; i < st_lsm6dsvx_odr_table[id].size; i++) { + sensor_odr = ST_LSM6DSVX_ODR_EXPAND( + st_lsm6dsvx_odr_table[id].odr_avl[i].hz, + st_lsm6dsvx_odr_table[id].odr_avl[i].uhz); + if (sensor_odr >= required_odr) + break; + } + + if (i == st_lsm6dsvx_odr_table[id].size) + return -EINVAL; + + *val = st_lsm6dsvx_odr_table[id].odr_avl[i].batch_val; + + return 0; +} + +static int +st_lsm6dsvx_set_hw_sensor_odr(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id, + int req_odr, int req_uodr) +{ + int err; + u8 val = 0; + + if (ST_LSM6DSVX_ODR_EXPAND(req_odr, req_uodr) > 0) { + err = st_lsm6dsvx_get_odr_val(id, req_odr, req_uodr, + &req_odr, &req_uodr, &val); + if (err < 0) + return err; + } + + err = st_lsm6dsvx_write_with_mask(hw, + st_lsm6dsvx_odr_table[id].reg.addr, + st_lsm6dsvx_odr_table[id].reg.mask, + val); + + return err < 0 ? err : 0; +} + +static u16 +st_lsm6dsvx_check_odr_dependency(struct st_lsm6dsvx_hw *hw, int odr, int uodr, + enum st_lsm6dsvx_sensor_id ref_id) +{ + struct st_lsm6dsvx_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + bool enable = ST_LSM6DSVX_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_lsm6dsvx_check_acc_odr_dependency(struct st_lsm6dsvx_sensor *sensor, + int req_odr, int req_uodr) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + enum st_lsm6dsvx_sensor_id id; + int odr = 0; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_acc_dep_sensor_list); i++) { + id = st_lsm6dsvx_acc_dep_sensor_list[i]; + if (!hw->iio_devs[id]) + continue; + + if (id == sensor->id) + continue; + + /* req_uodr not used */ + odr = st_lsm6dsvx_check_odr_dependency(hw, req_odr, + req_uodr, id); + if (odr != req_odr) + return 0; + } + + return odr; +} + +static int +st_lsm6dsvx_check_gyro_odr_dependency(struct st_lsm6dsvx_sensor *sensor, + int req_odr, int req_uodr) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + enum st_lsm6dsvx_sensor_id id; + int odr = 0; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_gyro_dep_sensor_list); i++) { + id = st_lsm6dsvx_gyro_dep_sensor_list[i]; + if (!hw->iio_devs[id]) + continue; + + if (id == sensor->id) + continue; + + /* req_uodr not used */ + odr = st_lsm6dsvx_check_odr_dependency(hw, req_odr, + req_uodr, id); + if (odr != req_odr) + return 0; + } + + return odr; +} +static int st_lsm6dsvx_set_odr(struct st_lsm6dsvx_sensor *sensor, + int req_odr, int req_uodr) +{ + enum st_lsm6dsvx_sensor_id id = sensor->id; + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err, odr; + + switch (id) { + case ST_LSM6DSVX_ID_QVAR: + case ST_LSM6DSVX_ID_EXT0: + case ST_LSM6DSVX_ID_EXT1: + case ST_LSM6DSVX_ID_FSM_0: + case ST_LSM6DSVX_ID_FSM_1: + case ST_LSM6DSVX_ID_FSM_2: + case ST_LSM6DSVX_ID_FSM_3: + case ST_LSM6DSVX_ID_FSM_4: + case ST_LSM6DSVX_ID_FSM_5: + case ST_LSM6DSVX_ID_FSM_6: + case ST_LSM6DSVX_ID_FSM_7: + case ST_LSM6DSVX_ID_MLC_0: + case ST_LSM6DSVX_ID_MLC_1: + case ST_LSM6DSVX_ID_MLC_2: + case ST_LSM6DSVX_ID_MLC_3: + case ST_LSM6DSVX_ID_TEMP: + case ST_LSM6DSVX_ID_STEP_COUNTER: + case ST_LSM6DSVX_ID_STEP_DETECTOR: + case ST_LSM6DSVX_ID_SIGN_MOTION: + case ST_LSM6DSVX_ID_TILT: + case ST_LSM6DSVX_ID_TAP: + case ST_LSM6DSVX_ID_DTAP: + case ST_LSM6DSVX_ID_WK: + case ST_LSM6DSVX_ID_FF: + case ST_LSM6DSVX_ID_SLPCHG: + case ST_LSM6DSVX_ID_6D: + case ST_LSM6DSVX_ID_ACC: + odr = st_lsm6dsvx_check_acc_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + return st_lsm6dsvx_set_hw_sensor_odr(hw, ST_LSM6DSVX_ID_ACC, + req_odr, req_uodr); + case ST_LSM6DSVX_ID_6X_GAME: + odr = st_lsm6dsvx_check_acc_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + err = st_lsm6dsvx_set_hw_sensor_odr(hw, ST_LSM6DSVX_ID_ACC, + req_odr, req_uodr); + if (err < 0) + return err; + + odr = st_lsm6dsvx_check_gyro_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + return st_lsm6dsvx_set_hw_sensor_odr(hw, ST_LSM6DSVX_ID_GYRO, + req_odr, req_uodr); + case ST_LSM6DSVX_ID_GYRO: + odr = st_lsm6dsvx_check_gyro_odr_dependency(sensor, req_odr, + req_uodr); + if (odr != req_odr) + return 0; + + return st_lsm6dsvx_set_hw_sensor_odr(hw, ST_LSM6DSVX_ID_GYRO, + req_odr, req_uodr); + default: + break; + } + + return 0; +} + +int +st_lsm6dsvx_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsvx_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_lsm6dsvx_sflp_set_enable(struct st_lsm6dsvx_sensor *sensor, bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 mask, fifo_mask; + int err; + + if (sensor->id != ST_LSM6DSVX_ID_6X_GAME) + return -EINVAL; + + fifo_mask = ST_LSM6DSVX_SFLP_GAME_FIFO_EN; + mask = ST_LSM6DSVX_SFLP_GAME_EN_MASK; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock; + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_EMB_FUNC_EN_A_ADDR, + mask, enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_EMB_FUNC_FIFO_EN_A_ADDR, + fifo_mask, enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsvx_set_sensor_batching_odr(sensor, enable); + +reset_page: + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, + 0); +unlock: + mutex_unlock(&hw->page_lock); + + return st_lsm6dsvx_sensor_set_enable(sensor, enable); +} + +static int +st_lsm6dsvx_read_oneshot(struct st_lsm6dsvx_sensor *sensor, u8 addr, int *val) +{ + int err, delay; + __le16 data; + + err = st_lsm6dsvx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsvx_read_locked(sensor->hw, addr, + (u8 *)&data, sizeof(data)); + if (err < 0) + return err; + + st_lsm6dsvx_sensor_set_enable(sensor, false); + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dsvx_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_lsm6dsvx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsvx_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_lsm6dsvx_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_LSM6DSVX_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_lsm6dsvx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsvx_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + int todr, tuodr; + u8 data; + + err = st_lsm6dsvx_get_odr_val(sensor->id, val, val2, + &todr, &tuodr, &data); + if (!err) { + sensor->odr = todr; + 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_LSM6DSVX_ID_GYRO: + case ST_LSM6DSVX_ID_ACC: + err = st_lsm6dsvx_set_odr(sensor, + sensor->odr, + sensor->uodr); + if (err < 0) + break; + + err = st_lsm6dsvx_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_lsm6dsvx_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsvx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dsvx_odr_table[id].size; i++) { + if (!st_lsm6dsvx_odr_table[id].odr_avl[i].hz) + continue; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lsm6dsvx_odr_table[id].odr_avl[i].hz, + st_lsm6dsvx_odr_table[id].odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lsm6dsvx_sysfs_scale_avail(struct device *dev, struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsvx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < st_lsm6dsvx_fs_table[id].size; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + st_lsm6dsvx_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static __maybe_unused int st_lsm6dsvx_reg_access(struct iio_dev *iio_dev, + unsigned int reg, + unsigned int writeval, + unsigned int *readval) +{ + struct st_lsm6dsvx_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_lsm6dsvx_of_get_pin(struct st_lsm6dsvx_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_lsm6dsvx_get_int_reg(struct st_lsm6dsvx_hw *hw, + u8 *drdy_reg, u8 *ef_irq_reg) +{ + int int_pin; + + if (st_lsm6dsvx_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_LSM6DSVX_REG_INT1_CTRL_ADDR; + break; + case 2: + *drdy_reg = ST_LSM6DSVX_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_lsm6dsvx_set_selftest(struct st_lsm6dsvx_sensor *sensor, int index) +{ + u8 mask; + + switch (sensor->id) { + case ST_LSM6DSVX_ID_ACC: + mask = ST_LSM6DSVX_ST_XL_MASK; + break; + case ST_LSM6DSVX_ID_GYRO: + mask = ST_LSM6DSVX_ST_G_MASK; + break; + default: + return -EINVAL; + } + + return st_lsm6dsvx_write_with_mask(sensor->hw, + ST_LSM6DSVX_REG_CTRL10_ADDR, mask, + st_lsm6dsvx_selftest_table[index].value); +} + +static ssize_t st_lsm6dsvx_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s\n", + st_lsm6dsvx_selftest_table[1].smode, + st_lsm6dsvx_selftest_table[2].smode); +} + +static ssize_t +st_lsm6dsvx_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsvx_sensor_id id = sensor->id; + int8_t result; + char *message; + + if (id != ST_LSM6DSVX_ID_ACC && + id != ST_LSM6DSVX_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_lsm6dsvx_selftest_sensor(struct st_lsm6dsvx_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_LSM6DSVX_ID_ACC: + reg = ST_LSM6DSVX_REG_OUTX_L_A_ADDR; + bitmask = ST_LSM6DSVX_XLDA_MASK; + break; + case ST_LSM6DSVX_ID_GYRO: + reg = ST_LSM6DSVX_REG_OUTX_L_G_ADDR; + bitmask = ST_LSM6DSVX_GDA_MASK; + break; + default: + return -EINVAL; + } + + /* set selftest normal mode */ + ret = st_lsm6dsvx_set_selftest(sensor, 0); + if (ret < 0) + return ret; + + ret = st_lsm6dsvx_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_lsm6dsvx_read_locked(sensor->hw, + ST_LSM6DSVX_REG_STATUS_REG_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_read_locked(sensor->hw, + ST_LSM6DSVX_REG_STATUS_REG_ADDR, + &status, sizeof(status)); + if (ret < 0) + goto selftest_failure; + + if (status & bitmask) { + ret = st_lsm6dsvx_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_lsm6dsvx_set_selftest(sensor, 0); + + return st_lsm6dsvx_sensor_set_enable(sensor, false); +} + +static ssize_t st_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + enum st_lsm6dsvx_sensor_id id = sensor->id; + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 drdy_reg, ef_irq_reg; + int ret, test; + int odr, uodr; + u32 gain; + + if (id != ST_LSM6DSVX_ID_ACC && + id != ST_LSM6DSVX_ID_GYRO) + return -EINVAL; + + for (test = 0; test < ARRAY_SIZE(st_lsm6dsvx_selftest_table); test++) { + if (strncmp(buf, st_lsm6dsvx_selftest_table[test].smode, + strlen(st_lsm6dsvx_selftest_table[test].smode)) == 0) + break; + } + + if (test == ARRAY_SIZE(st_lsm6dsvx_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_lsm6dsvx_get_int_reg(hw, &drdy_reg, &ef_irq_reg); + if (ret < 0) + goto restore_regs; + + ret = st_lsm6dsvx_write_with_mask(hw, drdy_reg, + ST_LSM6DSVX_INT_FIFO_TH_MASK, 0); + if (ret < 0) + goto restore_regs; + + gain = sensor->gain; + odr = sensor->odr; + uodr = sensor->uodr; + if (id == ST_LSM6DSVX_ID_ACC) { + /* set BDU = 1, FS = 4 g, ODR = 60 Hz */ + st_lsm6dsvx_set_full_scale(sensor, ST_LSM6DSVX_ACC_FS_4G_GAIN); + st_lsm6dsvx_set_odr(sensor, 60, 0); + } else { + /* set BDU = 1, ODR = 240 Hz, FS = 2000 dps */ + st_lsm6dsvx_set_full_scale(sensor, + ST_LSM6DSVX_GYRO_FS_2000_GAIN); + st_lsm6dsvx_set_odr(sensor, 240, 0); + } + + /* run test */ + st_lsm6dsvx_selftest_sensor(sensor, test); + +restore_regs: + /* restore configuration after test */ + st_lsm6dsvx_set_full_scale(sensor, gain); + st_lsm6dsvx_set_odr(sensor, odr, uodr); + st_lsm6dsvx_write_with_mask(hw, drdy_reg, + ST_LSM6DSVX_INT_FIFO_TH_MASK, 1); + +out_claim: + iio_device_release_direct_mode(iio_dev); + + return size; +} + +ssize_t st_lsm6dsvx_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%u\n", hw->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsvx_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dsvx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dsvx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444, + st_lsm6dsvx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsvx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, + st_lsm6dsvx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_lsm6dsvx_get_watermark, + st_lsm6dsvx_set_watermark, 0); +static IIO_DEVICE_ATTR(selftest_available, 0444, + st_lsm6dsvx_sysfs_get_selftest_available, + NULL, 0); +static IIO_DEVICE_ATTR(selftest, 0644, + st_lsm6dsvx_sysfs_get_selftest_status, + st_lsm6dsvx_sysfs_start_selftest, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsvx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsvx_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, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_acc_attribute_group = { + .attrs = st_lsm6dsvx_acc_attributes, +}; + +static const struct iio_info st_lsm6dsvx_acc_info = { + .attrs = &st_lsm6dsvx_acc_attribute_group, + .read_raw = st_lsm6dsvx_read_raw, + .write_raw_get_fmt = st_lsm6dsvx_write_raw_get_fmt, + .write_raw = st_lsm6dsvx_write_raw, + .debugfs_reg_access = st_lsm6dsvx_reg_access, +}; + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_gyro_attribute_group = { + .attrs = st_lsm6dsvx_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsvx_gyro_info = { + .attrs = &st_lsm6dsvx_gyro_attribute_group, + .read_raw = st_lsm6dsvx_read_raw, + .write_raw_get_fmt = st_lsm6dsvx_write_raw_get_fmt, + .write_raw = st_lsm6dsvx_write_raw, +}; + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_temp_attribute_group = { + .attrs = st_lsm6dsvx_temp_attributes, +}; + +static const struct iio_info st_lsm6dsvx_temp_info = { + .attrs = &st_lsm6dsvx_temp_attribute_group, + .read_raw = st_lsm6dsvx_read_raw, + .write_raw_get_fmt = st_lsm6dsvx_write_raw_get_fmt, + .write_raw = st_lsm6dsvx_write_raw, +}; + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_sflp_attribute_group = { + .attrs = st_lsm6dsvx_sflp_attributes, +}; + +static const struct iio_info st_lsm6dsvx_sflp_info = { + .attrs = &st_lsm6dsvx_sflp_attribute_group, + .read_raw = st_lsm6dsvx_read_raw, + .write_raw = st_lsm6dsvx_write_raw, +}; + +static const unsigned long st_lsm6dsvx_available_scan_masks[] = { + GENMASK(3, 0), 0x0 +}; + +static const unsigned long st_lsm6dsvx_temp_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int st_lsm6dsvx_reset_device(struct st_lsm6dsvx_hw *hw) +{ + int err; + + /* sw reset */ + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_CTRL3_ADDR, + ST_LSM6DSVX_SW_RESET_MASK, + 1); + if (err < 0) + return err; + + msleep(10); + + /* boot */ + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_CTRL3_ADDR, + ST_LSM6DSVX_BOOT_MASK, 1); + + msleep(50); + + return err; +} + +static int st_lsm6dsvx_init_device(struct st_lsm6dsvx_hw *hw) +{ + u8 drdy_reg, ef_irq_reg; + int err; + + /* latch interrupts */ + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_TAP_CFG0_ADDR, + ST_LSM6DSVX_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable Block Data Update */ + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_CTRL3_ADDR, + ST_LSM6DSVX_BDU_MASK, 1); + if (err < 0) + return err; + + /* init timestamp engine */ + err = st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_FUNCTIONS_ENABLE_ADDR, + ST_LSM6DSVX_TIMESTAMP_EN_MASK, 1); + if (err < 0) + return err; + + err = st_lsm6dsvx_get_int_reg(hw, &drdy_reg, &ef_irq_reg); + if (err < 0) + return err; + + /* enable DRDY MASK for filters settling time */ + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_CTRL4_ADDR, + ST_LSM6DSVX_DRDY_MASK, 1); + if (err < 0) + return err; + + /* enable FIFO watermak interrupt */ + return st_lsm6dsvx_write_with_mask(hw, drdy_reg, + ST_LSM6DSVX_INT_FIFO_TH_MASK, 1); +} + +static struct iio_dev * +st_lsm6dsvx_alloc_iiodev(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id) +{ + struct st_lsm6dsvx_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_LSM6DSVX_ID_ACC: + iio_dev->channels = st_lsm6dsvx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_acc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_accel", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_acc_info; + iio_dev->available_scan_masks = st_lsm6dsvx_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSVX_REG_FIFO_CTRL3_ADDR; + sensor->batch_reg.mask = ST_LSM6DSVX_BDR_XL_MASK; + sensor->max_watermark = ST_LSM6DSVX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsvx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsvx_odr_table[id].odr_avl[1].uhz; + sensor->gain = st_lsm6dsvx_fs_table[id].fs_avl[0].gain; + sensor->min_st = ST_LSM6DSVX_SELFTEST_ACCEL_MIN; + sensor->max_st = ST_LSM6DSVX_SELFTEST_ACCEL_MAX; + break; + case ST_LSM6DSVX_ID_GYRO: + iio_dev->channels = st_lsm6dsvx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_gyro_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_gyro", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_gyro_info; + iio_dev->available_scan_masks = st_lsm6dsvx_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSVX_REG_FIFO_CTRL3_ADDR; + sensor->batch_reg.mask = ST_LSM6DSVX_BDR_GY_MASK; + sensor->max_watermark = ST_LSM6DSVX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsvx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsvx_odr_table[id].odr_avl[1].uhz; + sensor->gain = st_lsm6dsvx_fs_table[id].fs_avl[1].gain; + sensor->min_st = ST_LSM6DSVX_SELFTEST_GYRO_MIN; + sensor->max_st = ST_LSM6DSVX_SELFTEST_GYRO_MAX; + break; + case ST_LSM6DSVX_ID_TEMP: + iio_dev->channels = st_lsm6dsvx_temp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_temp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_temp_info; + iio_dev->available_scan_masks = + st_lsm6dsvx_temp_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSVX_REG_FIFO_CTRL4_ADDR; + sensor->batch_reg.mask = ST_LSM6DSVX_ODR_T_BATCH_MASK; + sensor->max_watermark = ST_LSM6DSVX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsvx_odr_table[id].odr_avl[1].hz; + sensor->uodr = st_lsm6dsvx_odr_table[id].odr_avl[1].uhz; + sensor->gain = st_lsm6dsvx_fs_table[id].fs_avl[1].gain; + sensor->offset = ST_LSM6DSVX_TEMP_OFFSET; + break; + case ST_LSM6DSVX_ID_6X_GAME: + iio_dev->channels = st_lsm6dsvx_sflp_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_sflp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_gamerot", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_sflp_info; + iio_dev->available_scan_masks = st_lsm6dsvx_available_scan_masks; + + sensor->batch_reg.addr = ST_LSM6DSVX_REG_SFLP_ODR_ADDR; + sensor->batch_reg.mask = ST_LSM6DSVX_SFLP_GAME_ODR_MASK; + sensor->max_watermark = ST_LSM6DSVX_MAX_FIFO_DEPTH; + sensor->odr = st_lsm6dsvx_odr_table[id].odr_avl[3].hz; + sensor->uodr = st_lsm6dsvx_odr_table[id].odr_avl[3].uhz; + sensor->gain = 1; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + st_lsm6dsvx_set_full_scale(sensor, sensor->gain); + + return iio_dev; +} + +static void st_lsm6dsvx_disable_regulator_action(void *_data) +{ + struct st_lsm6dsvx_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static void st_lsm6dsvx_get_properties(struct st_lsm6dsvx_hw *hw) +{ + if (device_property_read_u32(hw->dev, "st,module_id", + &hw->module_id)) { + hw->module_id = 1; + } +} + +static int st_lsm6dsvx_power_enable(struct st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap) +{ + struct st_lsm6dsvx_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_lsm6dsvx_power_enable(hw); + if (err != 0) + return err; + + /* select register bank zero */ + err = st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK | + ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 0); + if (err < 0) + return err; + + err = st_lsm6dsvx_check_whoami(hw, hw_id); + if (err < 0) + return err; + + st_lsm6dsvx_get_properties(hw); + + err = st_lsm6dsvx_get_odr_calibration(hw); + if (err < 0) + return err; + + err = st_lsm6dsvx_reset_device(hw); + if (err < 0) + return err; + + err = st_lsm6dsvx_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_lsm6dsvx_main_sensor_list); i++) { + enum st_lsm6dsvx_sensor_id id = st_lsm6dsvx_main_sensor_list[i]; + + /* don't probe if sflp not supported */ + if (!hw->settings->st_sflp_probe && + (id == ST_LSM6DSVX_ID_6X_GAME)) + continue; + + hw->iio_devs[id] = st_lsm6dsvx_alloc_iiodev(hw, id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + if (!dev_fwnode(dev) || + device_property_read_bool(dev, "enable-sensor-hub")) { + err = st_lsm6dsvx_shub_probe(hw); + if (err < 0) + return err; + } + + /* allocate step counter before buffer setup because use FIFO */ + err = st_lsm6dsvx_probe_embfunc(hw); + if (err < 0) + return err; + + err = st_lsm6dsvx_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_lsm6dsvx_qvar_probe(hw); + if (err) + return err; + } + + if (hw->irq > 0) { + err = st_lsm6dsvx_buffers_setup(hw); + if (err < 0) + return err; + } + + if (st_lsm6dsvx_run_mlc_task(hw)) { + err = st_lsm6dsvx_mlc_probe(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LSM6DSVX_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_lsm6dsvx_run_mlc_task(hw)) { + err = st_lsm6dsvx_mlc_init_preload(hw); + if (err) + return err; + } + + device_init_wakeup(dev, + device_property_read_bool(dev, "wakeup-source")); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsvx_probe); + +int st_lsm6dsvx_remove(struct device *dev) +{ + struct st_lsm6dsvx_hw *hw = dev_get_drvdata(dev); + + if (!hw->iio_devs[ST_LSM6DSVX_ID_QVAR]) + return 0; + + return st_lsm6dsvx_qvar_remove(dev); +} +EXPORT_SYMBOL(st_lsm6dsvx_remove); + +static int __maybe_unused st_lsm6dsvx_suspend(struct device *dev) +{ + struct st_lsm6dsvx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsvx_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSVX_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_lsm6dsvx_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + + if (st_lsm6dsvx_is_fifo_enabled(hw)) + err = st_lsm6dsvx_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_lsm6dsvx_resume(struct device *dev) +{ + struct st_lsm6dsvx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsvx_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_LSM6DSVX_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_lsm6dsvx_set_odr(sensor, sensor->odr, sensor->uodr); + if (err < 0) + return err; + } + + if (st_lsm6dsvx_is_fifo_enabled(hw)) + err = st_lsm6dsvx_set_fifo_mode(hw, ST_LSM6DSVX_FIFO_CONT); + + return err < 0 ? err : 0; +} + +const struct dev_pm_ops st_lsm6dsvx_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsvx_suspend, st_lsm6dsvx_resume) +}; +EXPORT_SYMBOL(st_lsm6dsvx_pm_ops); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsvx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_embfunc.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_embfunc.c new file mode 100644 index 000000000000..c504501ac670 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_embfunc.c @@ -0,0 +1,570 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx embedded function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.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_lsm6dsvx_step_counter_channels[] = { + { + .type = IIO_STEP_COUNTER, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + ST_LSM6DSVX_EVENT_CHANNEL(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_lsm6dsvx_step_detector_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_STEP_DETECTOR, 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_lsm6dsvx_sign_motion_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(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_lsm6dsvx_tilt_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_TILT, thr), +}; + +static const unsigned long st_lsm6dsvx_embfunc_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int +st_lsm6dsvx_embfunc_set_enable(struct st_lsm6dsvx_sensor *sensor, + u8 mask, u8 irq_mask, bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSVX_REG_EMB_FUNC_INT1_ADDR : + ST_LSM6DSVX_REG_EMB_FUNC_INT2_ADDR; + int err; + + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock; + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_EMB_FUNC_EN_A_ADDR, + mask, + enable ? 1 : 0); + if (err < 0) + goto reset_page; + + err = __st_lsm6dsvx_write_with_mask(hw, int_reg, irq_mask, + enable ? 1 : 0); + +reset_page: + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsvx_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_lsm6dsvx_embfunc_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + int err; + + switch (sensor->id) { + case ST_LSM6DSVX_ID_STEP_DETECTOR: + err = st_lsm6dsvx_embfunc_set_enable(sensor, + ST_LSM6DSVX_REG_PEDO_EN_MASK, + ST_LSM6DSVX_INT_STEP_DETECTOR_MASK, + enable); + break; + case ST_LSM6DSVX_ID_SIGN_MOTION: + err = st_lsm6dsvx_embfunc_set_enable(sensor, + ST_LSM6DSVX_REG_SIGN_MOTION_EN_MASK, + ST_LSM6DSVX_INT_SIG_MOT_MASK, + enable); + break; + case ST_LSM6DSVX_ID_TILT: + err = st_lsm6dsvx_embfunc_set_enable(sensor, + ST_LSM6DSVX_REG_TILT_EN_MASK, + ST_LSM6DSVX_INT_TILT_MASK, + enable); + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +/** + * st_lsm6dsvx_reset_step_counter() - Reset Step Counter value [EMB_FUN] + * + * @iio_dev: IIO device + * + * return < 0 if error, 0 otherwise + */ +static int st_lsm6dsvx_reset_step_counter(struct iio_dev *iio_dev) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock_page; + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_EMB_FUNC_SRC_ADDR, + ST_LSM6DSVX_PEDO_RST_STEP_MASK, 1); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + +unlock_page: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/** + * st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_lsm6dsvx_embfunc_sensor_set_enable(sensor, state); + iio_device_release_direct_mode(iio_dev); + + return err; +} + +/** + * st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_sysfs_reset_step_counter, 0); + +static IIO_DEVICE_ATTR(hwfifo_stepc_watermark_max, 0444, + st_lsm6dsvx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_stepc_flush, 0200, NULL, + st_lsm6dsvx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_stepc_watermark, 0644, + st_lsm6dsvx_get_watermark, + st_lsm6dsvx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsvx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_step_counter_attribute_group = { + .attrs = st_lsm6dsvx_step_counter_attributes, +}; + +static const struct iio_info st_lsm6dsvx_step_counter_info = { + .attrs = &st_lsm6dsvx_step_counter_attribute_group, +}; + +static struct attribute *st_lsm6dsvx_step_detector_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_step_detector_attribute_group = { + .attrs = st_lsm6dsvx_step_detector_attributes, +}; + +static const struct iio_info st_lsm6dsvx_step_detector_info = { + .attrs = &st_lsm6dsvx_step_detector_attribute_group, + .read_event_config = st_lsm6dsvx_read_embfunc_config, + .write_event_config = st_lsm6dsvx_write_embfunc_config, +}; + +static struct attribute *st_lsm6dsvx_sign_motion_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_sign_motion_attribute_group = { + .attrs = st_lsm6dsvx_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6dsvx_sign_motion_info = { + .attrs = &st_lsm6dsvx_sign_motion_attribute_group, + .read_event_config = st_lsm6dsvx_read_embfunc_config, + .write_event_config = st_lsm6dsvx_write_embfunc_config, +}; + +static struct attribute *st_lsm6dsvx_tilt_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_tilt_attribute_group = { + .attrs = st_lsm6dsvx_tilt_attributes, +}; + +static const struct iio_info st_lsm6dsvx_tilt_info = { + .attrs = &st_lsm6dsvx_tilt_attribute_group, + .read_event_config = st_lsm6dsvx_read_embfunc_config, + .write_event_config = st_lsm6dsvx_write_embfunc_config, +}; + +static int st_lsm6dsvx_embfunc_init(struct st_lsm6dsvx_hw *hw) +{ + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSVX_REG_MD1_CFG_ADDR : + ST_LSM6DSVX_REG_MD2_CFG_ADDR; + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock_page; + + /* enable embedded function latched interrupt */ + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_PAGE_RW_ADDR, + ST_LSM6DSVX_EMB_FUNC_LIR_MASK, 1); + if (err < 0) + goto unlock_page; + + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + + /* enable embedded function interrupt by default */ + err = __st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_REG_INT_EMB_FUNC_MASK, + 1); +unlock_page: + mutex_unlock(&hw->page_lock); + + return err; +} + +static struct iio_dev * +st_lsm6dsvx_alloc_embfunc_iiodev(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id) +{ + struct st_lsm6dsvx_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_lsm6dsvx_embfunc_available_scan_masks; + + /* set main sensor odr to 26 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[2].hz; + switch (id) { + case ST_LSM6DSVX_ID_STEP_COUNTER: + iio_dev->channels = st_lsm6dsvx_step_counter_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_step_counter_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_stepc", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_step_counter_info; + break; + case ST_LSM6DSVX_ID_STEP_DETECTOR: + iio_dev->channels = st_lsm6dsvx_step_detector_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_step_detector_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_stepd", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_step_detector_info; + break; + case ST_LSM6DSVX_ID_SIGN_MOTION: + iio_dev->channels = st_lsm6dsvx_sign_motion_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_sign_motion_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sigmot", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_sign_motion_info; + break; + case ST_LSM6DSVX_ID_TILT: + iio_dev->channels = st_lsm6dsvx_tilt_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_tilt_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tilt", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_tilt_info; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/** + * st_lsm6dsvx_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_lsm6dsvx_step_counter_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err; + + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, + 1); + if (err < 0) + goto unlock; + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_EMB_FUNC_EN_A_ADDR, + ST_LSM6DSVX_REG_PEDO_EN_MASK, + enable); + if (err < 0) + goto reset_page; + + /* enable step counter batching in fifo */ + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_EMB_FUNC_FIFO_EN_A_ADDR, + ST_LSM6DSVX_STEP_COUNTER_FIFO_EN_MASK, + enable); + +reset_page: + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); +unlock: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * st_lsm6dsvx_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_lsm6dsvx_embfunc_handler_thread(struct st_lsm6dsvx_hw *hw) +{ + if (hw->enable_mask & (BIT(ST_LSM6DSVX_ID_STEP_DETECTOR) | + BIT(ST_LSM6DSVX_ID_SIGN_MOTION) | + BIT(ST_LSM6DSVX_ID_TILT))) { + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + err = st_lsm6dsvx_read_locked(hw, + ST_LSM6DSVX_REG_EMB_FUNC_STATUS_MAINPAGE_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* embedded function sensors */ + if (status & ST_LSM6DSVX_IS_STEP_DET_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_STEP_DETECTOR]; + event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, + -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, + iio_get_time_ns(iio_dev)); + } + + if (status & ST_LSM6DSVX_IS_SIGMOT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_SIGN_MOTION]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSVX_IS_TILT_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_TILT]; + event = IIO_UNMOD_EVENT_CODE(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_lsm6dsvx_probe_embfunc() - Allocate IIO embedded function device + * + * @hw: ST IMU MEMS hw instance. + * + * return 0 or < 0 for error + */ +int st_lsm6dsvx_probe_embfunc(struct st_lsm6dsvx_hw *hw) +{ + enum st_lsm6dsvx_sensor_id id; + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_embfunc_sensor_list); + i++) { + + id = st_lsm6dsvx_embfunc_sensor_list[i]; + hw->iio_devs[id] = st_lsm6dsvx_alloc_embfunc_iiodev(hw, + id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + return st_lsm6dsvx_embfunc_init(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_events.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_events.c new file mode 100644 index 000000000000..f3aa39046a61 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_events.c @@ -0,0 +1,1032 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx events function sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +static const struct st_lsm6dsvx_ff_th st_lsm6dsvx_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_lsm6dsvx_6D_th st_lsm6dsvx_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_lsm6dsvx_event_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static const struct iio_chan_spec st_lsm6dsvx_wk_channels[] = { + { + .type = 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_lsm6dsvx_ff_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_lsm6dsvx_sc_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_GESTURE, thr), +}; + +static const struct iio_chan_spec st_lsm6dsvx_6D_channels[] = { + { + .type = 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_lsm6dsvx_tap_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_TAP, thr), +}; + +static const struct iio_chan_spec st_lsm6dsvx_dtap_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_TAP_TAP, thr), +}; + +/* + * st_lsm6dsvx_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_lsm6dsvx_set_wake_up_thershold(struct st_lsm6dsvx_hw *hw, + int th_ug) +{ + struct st_lsm6dsvx_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_lsm6dsvx_read_with_mask(hw, + hw->fs_table[ST_LSM6DSVX_ID_ACC].fs_avl[0].reg.addr, + hw->fs_table[ST_LSM6DSVX_ID_ACC].fs_avl[0].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_LSM6DSVX_WK_THS_MASK >> __ffs(ST_LSM6DSVX_WK_THS_MASK); + if (val > max_th) + val = max_th; + + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSVX_WK_THS_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[0] = th_ug; + + return 0; +} + +/* + * st_lsm6dsvx_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_lsm6dsvx_set_wake_up_duration(struct st_lsm6dsvx_hw *hw, + int dur_ms) +{ + struct st_lsm6dsvx_sensor *sensor; + struct iio_dev *iio_dev; + int i, tmp, sensor_odr, err; + u8 val, odr_xl, max_dur; + + err = st_lsm6dsvx_read_with_mask(hw, + hw->odr_table[ST_LSM6DSVX_ID_ACC].reg.addr, + hw->odr_table[ST_LSM6DSVX_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_LSM6DSVX_ID_ACC].odr_avl[2].val; + } + + for (i = 0; i < hw->odr_table[ST_LSM6DSVX_ID_ACC].size; i++) { + if (odr_xl == + hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[i].val) + break; + } + + if (i == hw->odr_table[ST_LSM6DSVX_ID_ACC].size) + return -EINVAL; + + + sensor_odr = ST_LSM6DSVX_ODR_EXPAND( + hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[i].hz, + hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[i].uhz); + + tmp = dur_ms / (1000000 / (sensor_odr / 1000)); + val = (u8)tmp; + max_dur = ST_LSM6DSVX_WAKE_DUR_MASK >> + __ffs(ST_LSM6DSVX_WAKE_DUR_MASK); + if (val > max_dur) + val = max_dur; + + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_WAKE_UP_DUR_ADDR, + ST_LSM6DSVX_WAKE_DUR_MASK, val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_WK]; + sensor = iio_priv(iio_dev); + sensor->conf[1] = dur_ms; + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[i].hz; + + return 0; +} + +/* + * st_lsm6dsvx_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_lsm6dsvx_set_freefall_threshold(struct st_lsm6dsvx_hw *hw, + int th_mg) +{ + struct st_lsm6dsvx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_free_fall_threshold); i++) { + if (th_mg >= st_lsm6dsvx_free_fall_threshold[i].mg) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsvx_free_fall_threshold)) + return -EINVAL; + + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_FREE_FALL_ADDR, + ST_LSM6DSVX_FF_THS_MASK, + st_lsm6dsvx_free_fall_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_FF]; + sensor = iio_priv(iio_dev); + sensor->conf[2] = th_mg; + + return 0; +} + +/* + * st_lsm6dsvx_set_6D_threshold - set 6D threshold detection in degrees + * + * @hw - ST IMU MEMS hw instance + * @deg - 6D threshold in degrees + */ +static int st_lsm6dsvx_set_6D_threshold(struct st_lsm6dsvx_hw *hw, + int deg) +{ + struct st_lsm6dsvx_sensor *sensor; + struct iio_dev *iio_dev; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_6D_threshold); i++) { + if (deg >= st_lsm6dsvx_6D_threshold[i].deg) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsvx_6D_threshold)) + return -EINVAL; + + err = st_lsm6dsvx_write_with_mask(hw, ST_LSM6DSVX_REG_TAP_THS_6D_ADDR, + ST_LSM6DSVX_SIXD_THS_MASK, + st_lsm6dsvx_6D_threshold[i].val); + if (err < 0) + return err; + + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_6D]; + sensor = iio_priv(iio_dev); + sensor->conf[3] = deg; + + return 0; +} + +/* + * st_lsm6dsvx_init_tap - initialize tap detection to default value + * + * @hw - ST IMU MEMS hw instance + */ +static int st_lsm6dsvx_init_tap(struct st_lsm6dsvx_hw *hw) +{ + int err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_TAP_CFG0_ADDR, + ST_LSM6DSVX_REG_TAP_EN_MASK, + FIELD_PREP(ST_LSM6DSVX_REG_TAP_EN_MASK, 0x07)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_TAP_CFG1_ADDR, + ST_LSM6DSVX_TAP_THS_X_MASK, + FIELD_PREP(ST_LSM6DSVX_TAP_THS_X_MASK, 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_TAP_CFG2_ADDR, + ST_LSM6DSVX_TAP_THS_Y_MASK, + FIELD_PREP(ST_LSM6DSVX_TAP_THS_Y_MASK, 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_TAP_THS_6D_ADDR, + ST_LSM6DSVX_TAP_THS_Z_MASK, + FIELD_PREP(ST_LSM6DSVX_TAP_THS_Z_MASK, 0x09)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_TAP_DUR_ADDR, + ST_LSM6DSVX_SHOCK_MASK, + FIELD_PREP(ST_LSM6DSVX_SHOCK_MASK, 0x02)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, + ST_LSM6DSVX_REG_TAP_DUR_ADDR, + ST_LSM6DSVX_QUIET_MASK, + FIELD_PREP(ST_LSM6DSVX_QUIET_MASK, 0x01)); + + return err < 0 ? err : 0; +} + +static int +st_lsm6dsvx_event_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + int err, eint = !!enable; + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 int_reg = hw->int_pin == 1 ? ST_LSM6DSVX_REG_MD1_CFG_ADDR : + ST_LSM6DSVX_REG_MD2_CFG_ADDR; + + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + switch (sensor->id) { + case ST_LSM6DSVX_ID_WK: + err = st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_INT_WU_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSVX_ID_FF: + err = st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_INT_FF_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSVX_ID_SLPCHG: + err = st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_INT_SLEEP_CHANGE_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSVX_ID_6D: + err = st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_INT_6D_MASK, + eint); + if (err < 0) + return err; + break; + case ST_LSM6DSVX_ID_TAP: + err = st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_TAP_IA_MASK, + eint); + if (err < 0) + return err; + + err = st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSVX_SINGLE_DOUBLE_TAP_MASK, + 0); + if (err < 0) + return err; + break; + case ST_LSM6DSVX_ID_DTAP: + err = st_lsm6dsvx_write_with_mask(hw, int_reg, + ST_LSM6DSVX_INT_DOUBLE_TAP_MASK, + eint); + if (err < 0) + return err; + + err = st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_WAKE_UP_THS_ADDR, + ST_LSM6DSVX_SINGLE_DOUBLE_TAP_MASK, + 1); + if (err < 0) + return err; + break; + default: + err = -EINVAL; + break; + } + + if (err >= 0) { + err = st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_FUNCTIONS_ENABLE_ADDR, + ST_LSM6DSVX_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_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +static int +st_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_lsm6dsvx_event_sensor_set_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_lsm6dsvx_wakeup_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[0]); +} + +static ssize_t +st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_wakeup_duration_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[1]); +} + +static ssize_t +st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_freefall_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[2]); +} + +static ssize_t +st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_6D_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->conf[3]); +} + +static ssize_t +st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_wakeup_threshold_get, + st_lsm6dsvx_wakeup_threshold_set, 0); + +static IIO_DEVICE_ATTR(wakeup_duration, 0644, + st_lsm6dsvx_wakeup_duration_get, + st_lsm6dsvx_wakeup_duration_set, 0); + +static IIO_DEVICE_ATTR(freefall_threshold, 0644, + st_lsm6dsvx_freefall_threshold_get, + st_lsm6dsvx_freefall_threshold_set, 0); + +static IIO_DEVICE_ATTR(sixd_threshold, 0644, + st_lsm6dsvx_6D_threshold_get, + st_lsm6dsvx_6D_threshold_set, 0); + +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsvx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_wk_attribute_group = { + .attrs = st_lsm6dsvx_wk_attributes, +}; + +static const struct iio_info st_lsm6dsvx_wk_info = { + .attrs = &st_lsm6dsvx_wk_attribute_group, +}; + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_ff_attribute_group = { + .attrs = st_lsm6dsvx_ff_attributes, +}; + +static const struct iio_info st_lsm6dsvx_ff_info = { + .attrs = &st_lsm6dsvx_ff_attribute_group, + .read_event_config = st_lsm6dsvx_read_event_config, + .write_event_config = st_lsm6dsvx_write_event_config, +}; + +static struct attribute *st_lsm6dsvx_sc_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_sc_attribute_group = { + .attrs = st_lsm6dsvx_sc_attributes, +}; + +static const struct iio_info st_lsm6dsvx_sc_info = { + .attrs = &st_lsm6dsvx_sc_attribute_group, + .read_event_config = st_lsm6dsvx_read_event_config, + .write_event_config = st_lsm6dsvx_write_event_config, +}; + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_6D_attribute_group = { + .attrs = st_lsm6dsvx_6D_attributes, +}; + +static const struct iio_info st_lsm6dsvx_6D_info = { + .attrs = &st_lsm6dsvx_6D_attribute_group, +}; + +static struct attribute *st_lsm6dsvx_tap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_tap_attribute_group = { + .attrs = st_lsm6dsvx_tap_attributes, +}; + +static const struct iio_info st_lsm6dsvx_tap_info = { + .attrs = &st_lsm6dsvx_tap_attribute_group, + .read_event_config = st_lsm6dsvx_read_event_config, + .write_event_config = st_lsm6dsvx_write_event_config, +}; + +static struct attribute *st_lsm6dsvx_dtap_attributes[] = { + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsvx_dtap_attribute_group = { + .attrs = st_lsm6dsvx_dtap_attributes, +}; + +static const struct iio_info st_lsm6dsvx_dtap_info = { + .attrs = &st_lsm6dsvx_dtap_attribute_group, + .read_event_config = st_lsm6dsvx_read_event_config, + .write_event_config = st_lsm6dsvx_write_event_config, +}; + +static struct iio_dev * +st_lsm6dsvx_alloc_event_iiodev(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id) +{ + struct st_lsm6dsvx_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_lsm6dsvx_event_available_scan_masks; + + switch (id) { + case ST_LSM6DSVX_ID_WK: + iio_dev->channels = st_lsm6dsvx_wk_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_wk_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_wk", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_wk_info; + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSVX_ID_FF: + iio_dev->channels = st_lsm6dsvx_ff_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_ff_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ff", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_ff_info; + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSVX_ID_SLPCHG: + iio_dev->channels = st_lsm6dsvx_sc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_sc_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_sc", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_sc_info; + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSVX_ID_6D: + iio_dev->channels = st_lsm6dsvx_6D_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_6D_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_6d", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_6D_info; + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[3].hz; + break; + case ST_LSM6DSVX_ID_TAP: + iio_dev->channels = st_lsm6dsvx_tap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_tap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_tap", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_tap_info; + + /* require main sensor odr > 400 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[7].hz; + break; + case ST_LSM6DSVX_ID_DTAP: + iio_dev->channels = st_lsm6dsvx_dtap_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_dtap_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_dtap", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_dtap_info; + + /* require main sensor odr > 400 Hz */ + sensor->odr = hw->odr_table[ST_LSM6DSVX_ID_ACC].odr_avl[7].hz; + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +/** + * st_lsm6dsvx_event_handler() - Detect embedded low power event + * + * @hw: ST IMU MEMS hw instance. + * + * return IRQ_HANDLED. + * + * NOTE: Uses page_lock through the st_lsm6dsvx_read_locked. + */ +int st_lsm6dsvx_event_handler(struct st_lsm6dsvx_hw *hw) +{ + struct iio_dev *iio_dev; + u8 status; + s64 event; + int err; + + if (hw->enable_mask & + (BIT(ST_LSM6DSVX_ID_WK) | BIT(ST_LSM6DSVX_ID_FF) | + BIT(ST_LSM6DSVX_ID_SLPCHG) | + BIT(ST_LSM6DSVX_ID_6D) | BIT(ST_LSM6DSVX_ID_TAP) | + BIT(ST_LSM6DSVX_ID_DTAP))) { + err = st_lsm6dsvx_read_locked(hw, + ST_LSM6DSVX_REG_ALL_INT_SRC_ADDR, + &status, sizeof(status)); + if (err < 0) + return IRQ_HANDLED; + + /* base function sensors */ + if (status & ST_LSM6DSVX_TAP_IA_MASK) { + if (BIT(ST_LSM6DSVX_ID_TAP)) { + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_TAP]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSVX_ID_DTAP]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSVX_FF_IA_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_FF]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSVX_WU_IA_MASK) { + struct st_lsm6dsvx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_WK]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + + if (status & ST_LSM6DSVX_SLEEP_CHANGE_MASK) { + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_SLPCHG]; + event = IIO_UNMOD_EVENT_CODE(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_LSM6DSVX_D6D_IA_MASK) { + struct st_lsm6dsvx_sensor *sensor; + + iio_dev = hw->iio_devs[ST_LSM6DSVX_ID_6D]; + sensor = iio_priv(iio_dev); + iio_trigger_poll_chained(sensor->trig); + } + } + + return IRQ_HANDLED; +} + +static inline int st_lsm6dsvx_get_6D(struct st_lsm6dsvx_hw *hw, u8 *out) +{ + return st_lsm6dsvx_read_with_mask(hw, ST_LSM6DSVX_REG_D6D_SRC_ADDR, + ST_LSM6DSVX_D6D_EVENT_MASK, out); +} + +static inline int st_lsm6dsvx_get_wk(struct st_lsm6dsvx_hw *hw, u8 *out) +{ + return st_lsm6dsvx_read_with_mask(hw, + ST_LSM6DSVX_REG_WAKE_UP_SRC_ADDR, + ST_LSM6DSVX_WAKE_UP_EVENT_MASK, out); +} + +static irqreturn_t st_lsm6dsvx_6D_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + st_lsm6dsvx_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_lsm6dsvx_wk_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + st_lsm6dsvx_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_lsm6dsvx_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig); + struct st_lsm6dsvx_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_lsm6dsvx_trigger_ops = { + .set_trigger_state = &st_lsm6dsvx_trig_set_state, +}; + +static int st_lsm6dsvx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsvx_event_sensor_set_enable(iio_priv(iio_dev), + true); +} + +static int st_lsm6dsvx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsvx_event_sensor_set_enable(iio_priv(iio_dev), + false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsvx_buffer_ops = { + .preenable = st_lsm6dsvx_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_lsm6dsvx_buffer_postdisable, +}; + +static int +st_lsm6dsvx_config_default_events(struct st_lsm6dsvx_hw *hw) +{ + int err; + + /* set default wake-up thershold to 93750 ug */ + err = st_lsm6dsvx_set_wake_up_thershold(hw, 93750); + if (err < 0) + return err; + + /* set default wake-up duration to 0 */ + err = st_lsm6dsvx_set_wake_up_duration(hw, 0); + if (err < 0) + return err; + + /* set default FF threshold to 312 mg */ + err = st_lsm6dsvx_set_freefall_threshold(hw, 312); + if (err < 0) + return err; + + /* set default 6D threshold to 60 degrees */ + err = st_lsm6dsvx_set_6D_threshold(hw, 60); + if (err < 0) + return err; + + return st_lsm6dsvx_init_tap(hw); +} + +int st_lsm6dsvx_probe_event(struct st_lsm6dsvx_hw *hw) +{ + struct st_lsm6dsvx_sensor *sensor; + struct iio_dev *iio_dev; + irqreturn_t (*pthread[ST_LSM6DSVX_ID_MAX - ST_LSM6DSVX_ID_WK])(int irq, void *p) = { + [0] = st_lsm6dsvx_wk_handler_thread, + [1] = st_lsm6dsvx_6D_handler_thread, + /* add here all other trigger handler funcions */ + }; + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_event_sensor_list); + i++) { + enum st_lsm6dsvx_sensor_id id = + st_lsm6dsvx_event_sensor_list[i]; + + hw->iio_devs[id] = st_lsm6dsvx_alloc_event_iiodev(hw, + id); + if (!hw->iio_devs[id]) + return -ENOMEM; + } + + /* configure trigger sensors */ + for (i = 0; + i < ARRAY_SIZE(st_lsm6dsvx_event_trigger_sensor_list); + i++) { + enum st_lsm6dsvx_sensor_id id = + st_lsm6dsvx_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_LSM6DSVX_ID_WK], + &st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_config_default_events(hw); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_hwtimestamp.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_hwtimestamp.c new file mode 100644 index 000000000000..116561f5e281 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_hwtimestamp.c @@ -0,0 +1,116 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx hwtimestamp library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +#define ST_LSM6DSVX_TSYNC_OFFSET_NS (300 * 1000LL) + +static void st_lsm6dsvx_read_hw_timestamp(struct st_lsm6dsvx_hw *hw) +{ + s64 timestamp_hw_global; + s64 eventLSB, eventMSB; + __le32 timestamp_hw; + s64 timestamp_cpu; + __le32 tmp; + int err; + + err = st_lsm6dsvx_read_locked(hw, ST_LSM6DSVX_REG_TIMESTAMP0_ADDR, + (u8 *)×tamp_hw, + sizeof(timestamp_hw)); + if (err < 0) + return; + + timestamp_cpu = iio_get_time_ns(hw->iio_devs[0]) - + ST_LSM6DSVX_TSYNC_OFFSET_NS; + + eventLSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 0, + IIO_EV_TYPE_TIME_SYNC, 0, 0, 0); + eventMSB = IIO_EVENT_CODE(IIO_COUNT, 0, 0, 1, + 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_LSM6DSVX_ID_GYRO)) { + iio_push_event(hw->iio_devs[ST_LSM6DSVX_ID_GYRO], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSVX_ID_GYRO], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSVX_ID_ACC)) { + iio_push_event(hw->iio_devs[ST_LSM6DSVX_ID_ACC], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSVX_ID_ACC], eventMSB, + timestamp_cpu); + } + if (hw->enable_mask & BIT_ULL(ST_LSM6DSVX_ID_TEMP)) { + iio_push_event(hw->iio_devs[ST_LSM6DSVX_ID_TEMP], eventLSB, + timestamp_cpu); + iio_push_event(hw->iio_devs[ST_LSM6DSVX_ID_TEMP], eventMSB, + timestamp_cpu); + } + + if (hw->timesync_c < 6) + hw->timesync_c++; + else + hw->timesync_ktime = ktime_set(0, ST_LSM6DSVX_DEFAULT_KTIME); +} + +static void st_lsm6dsvx_timesync_fn(struct work_struct *work) +{ + struct st_lsm6dsvx_hw *hw = container_of(work, struct st_lsm6dsvx_hw, + timesync_work); + + st_lsm6dsvx_read_hw_timestamp(hw); +} + +static enum hrtimer_restart st_lsm6dsvx_timer_fn(struct hrtimer *timer) +{ + struct st_lsm6dsvx_hw *hw; + + hw = container_of(timer, struct st_lsm6dsvx_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_lsm6dsvx_hwtimesync_init(struct st_lsm6dsvx_hw *hw) +{ + hw->timesync_c = 0; + hw->timesync_ktime = ktime_set(0, ST_LSM6DSVX_DEFAULT_KTIME); + hrtimer_init(&hw->timesync_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + hw->timesync_timer.function = st_lsm6dsvx_timer_fn; + + spin_lock_init(&hw->hwtimestamp_lock); + hw->hw_timestamp_global = 0; + + hw->timesync_workqueue = create_singlethread_workqueue("st_lsm6dsvx_workqueue"); + if (!hw->timesync_workqueue) + return -ENOMEM; + + INIT_WORK(&hw->timesync_work, st_lsm6dsvx_timesync_fn); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsvx_hwtimesync_init); diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i2c.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i2c.c new file mode 100644 index 000000000000..c21307a7248f --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i2c.c @@ -0,0 +1,79 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +static const struct regmap_config st_lsm6dsvx_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_probe(&client->dev, client->irq, hw_id, regmap); +} + +static int st_lsm6dsvx_i2c_remove(struct i2c_client *client) +{ + return st_lsm6dsvx_remove(&client->dev); +} + +static const struct of_device_id st_lsm6dsvx_i2c_of_match[] = { + { + .compatible = "st," ST_LSM6DSV_DEV_NAME, + .data = (void *)ST_LSM6DSV_ID, + }, + { + .compatible = "st," ST_LSM6DSV16X_DEV_NAME, + .data = (void *)ST_LSM6DSVX_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsvx_i2c_of_match); + +static const struct i2c_device_id st_lsm6dsvx_i2c_id_table[] = { + { ST_LSM6DSV_DEV_NAME, ST_LSM6DSV_ID }, + { ST_LSM6DSV16X_DEV_NAME, ST_LSM6DSVX_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsvx_i2c_id_table); + +static struct i2c_driver st_lsm6dsvx_driver = { + .driver = { + .name = "st_lsm6dsvx_i2c", + .pm = &st_lsm6dsvx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsvx_i2c_of_match), + }, + .probe = st_lsm6dsvx_i2c_probe, + .remove = st_lsm6dsvx_i2c_remove, + .id_table = st_lsm6dsvx_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsvx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsvx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i3c.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i3c.c new file mode 100644 index 000000000000..20989190f11b --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_i3c.c @@ -0,0 +1,58 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx i3c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +static const struct i3c_device_id st_lsm6dsvx_i3c_ids[] = { + I3C_DEVICE(0x0104, ST_LSM6DSVX_WHOAMI_VAL, (void *)ST_LSM6DSVX_ID), + {}, +}; +MODULE_DEVICE_TABLE(i3c, st_lsm6dsvx_i3c_ids); + +static int st_lsm6dsvx_i3c_probe(struct i3c_device *i3cdev) +{ + struct regmap_config st_lsm6dsvx_i3c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + }; + const struct i3c_device_id *id = + i3c_device_match_id(i3cdev, st_lsm6dsvx_i3c_ids); + struct regmap *regmap; + + regmap = devm_regmap_init_i3c(i3cdev, &st_lsm6dsvx_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_lsm6dsvx_probe(&i3cdev->dev, 0, (uintptr_t)id->data, regmap); +} + +static struct i3c_driver st_lsm6dsvx_driver = { + .driver = { + .name = "st_lsm6dsvx_i3c", + .pm = &st_lsm6dsvx_pm_ops, + }, + .probe = st_lsm6dsvx_i3c_probe, + .id_table = st_lsm6dsvx_i3c_ids, +}; +module_i3c_driver(st_lsm6dsvx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsvx i3c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_mlc.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_mlc.c new file mode 100644 index 000000000000..84ea18425317 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_mlc.c @@ -0,0 +1,827 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx machine learning core driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsvx.h" + +#define ST_LSM6DSVX_MLC_LOADER_VERSION "0.3" + +/* number of machine learning core available on device hardware */ +#define ST_LSM6DSVX_MLC_MAX_NUMBER 4 +#define ST_LSM6DSVX_FSM_MAX_NUMBER 8 + +#ifdef CONFIG_IIO_LSM6DSVX_MLC_BUILTIN_FIRMWARE +static const u8 st_lsm6dsvx_mlc_fw[] = { + #include "st_lsm6dsvx_mlc.fw" +}; +DECLARE_BUILTIN_FIRMWARE(LSM6DSVX_MLC_FIRMWARE_NAME, + st_lsm6dsvx_mlc_fw); +#else /* CONFIG_IIO_LSM6DSVX_MLC_BUILTIN_FIRMWARE */ +#define LSM6DSVX_MLC_FIRMWARE_NAME "st_lsm6dsvx_mlc.bin" +#endif /* CONFIG_IIO_LSM6DSVX_MLC_BUILTIN_FIRMWARE */ + +#ifdef CONFIG_IIO_ST_LSM6DSVX_MLC_PRELOAD +#include "st_lsm6dsvx_preload_mlc.h" +#endif /* CONFIG_IIO_ST_LSM6DSVX_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, +}; + +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_lsm6dsvx_mlc_alloc_iio_dev(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id); + +static const unsigned long st_lsm6dsvx_mlc_available_scan_masks[] = { + 0x1, 0x0 +}; + +static inline int +st_lsm6dsvx_read_page_locked(struct st_lsm6dsvx_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 1); + err = regmap_bulk_read(hw->regmap, addr, val, len); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsvx_write_page_locked(struct st_lsm6dsvx_hw *hw, unsigned int addr, + unsigned int *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 1); + err = regmap_bulk_write(hw->regmap, addr, val, len); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsvx_update_page_bits_locked(struct st_lsm6dsvx_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 1); + err = regmap_update_bits(hw->regmap, addr, mask, val); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + mutex_unlock(&hw->page_lock); + + return err; +} + +static int +st_lsm6dsvx_mlc_enable_sensor(struct st_lsm6dsvx_sensor *sensor, bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + int i, id, err = 0; + + /* enable acc sensor as trigger */ + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + if (sensor->status == ST_LSM6DSVX_MLC_ENABLED) { + int value; + + value = enable ? hw->mlc_config->mlc_int_mask : 0; + err = st_lsm6dsvx_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_lsm6dsvx_update_page_bits_locked(hw, + ST_LSM6DSVX_REG_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSVX_MLC_EN_MASK, + ST_LSM6DSVX_SHIFT_VAL(enable, + ST_LSM6DSVX_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_LSM6DSVX_FSM_ENABLED) { + int value; + + value = enable ? hw->mlc_config->fsm_int_mask : 0; + err = st_lsm6dsvx_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_LSM6DSVX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsvx_fsm_sensor_list[i]; + if (hw->enable_mask & BIT(id)) + break; + } + + /* check for any other fsm already enabled */ + if (enable || i == ST_LSM6DSVX_FSM_MAX_NUMBER) { + err = st_lsm6dsvx_update_page_bits_locked(hw, + ST_LSM6DSVX_REG_EMB_FUNC_EN_B_ADDR, + ST_LSM6DSVX_FSM_EN_MASK, + ST_LSM6DSVX_SHIFT_VAL(enable, + ST_LSM6DSVX_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_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsvx_mlc_enable_sensor(sensor, state); +} + +static int +st_lsm6dsvx_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_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsvx_hw *hw = sensor->hw; + + return !!(hw->enable_mask & BIT(sensor->id)); +} + +/* + * st_lsm6dsvx_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_lsm6dsvx_verify_mlc_fsm_support(const struct firmware *fw, + struct st_lsm6dsvx_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_LSM6DSVX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = true; + } else if (reg == ST_LSM6DSVX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ~ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSVX_REG_MLC_INT1_ADDR: + case ST_LSM6DSVX_REG_MLC_INT2_ADDR: + if (!hw->settings->st_mlc_probe) + return -ENODEV; + break; + case ST_LSM6DSVX_REG_FSM_INT1_ADDR: + case ST_LSM6DSVX_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_lsm6dsvx_program_mlc(const struct firmware *fw, + struct st_lsm6dsvx_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_LSM6DSVX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = true; + } else if (reg == ST_LSM6DSVX_REG_FUNC_CFG_ACCESS_ADDR && + (val & ~ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK)) { + stmc_page = false; + } else if (stmc_page) { + switch (reg) { + case ST_LSM6DSVX_REG_MLC_INT1_ADDR: + case ST_LSM6DSVX_REG_MLC_INT2_ADDR: + mlc_int |= val; + mlc_num++; + skip = 1; + break; + case ST_LSM6DSVX_REG_FSM_INT1_ADDR: + case ST_LSM6DSVX_REG_FSM_INT2_ADDR: + fsm_int |= val; + fsm_num++; + skip = 1; + break; + case ST_LSM6DSVX_REG_EMB_FUNC_EN_B_ADDR: + skip = 1; + break; + default: + break; + } + } else if (reg == ST_LSM6DSVX_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_LSM6DSVX_MLC_MAX_NUMBER || + fsm_num >= ST_LSM6DSVX_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_LSM6DSVX_REG_MLC_INT1_ADDR : + ST_LSM6DSVX_REG_MLC_INT2_ADDR); + + hw->mlc_config->status |= ST_LSM6DSVX_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_LSM6DSVX_REG_FSM_INT1_ADDR : + ST_LSM6DSVX_REG_FSM_INT2_ADDR); + + hw->mlc_config->status |= ST_LSM6DSVX_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_lsm6dsvx_mlc_update(const struct firmware *fw, void *context) +{ + struct st_lsm6dsvx_hw *hw = context; + enum st_lsm6dsvx_sensor_id id; + int ret, i; + + if (!fw) { + dev_err(hw->dev, "could not get binary firmware\n"); + + return; + } + + ret = st_lsm6dsvx_verify_mlc_fsm_support(fw, hw); + if (ret) { + dev_err(hw->dev, "invalid file format for device\n"); + + return; + } + + ret = st_lsm6dsvx_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_LSM6DSVX_MLC_MAX_NUMBER; i++) { + if (mlc_mask & BIT(i)) { + id = st_lsm6dsvx_mlc_sensor_list[i]; + hw->iio_devs[id] = st_lsm6dsvx_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_LSM6DSVX_FSM_MAX_NUMBER; i++) { + if (fsm_mask & BIT(i)) { + id = st_lsm6dsvx_fsm_sensor_list[i]; + hw->iio_devs[id] = st_lsm6dsvx_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_lsm6dsvx_mlc_flush_single(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id) +{ + struct st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_mlc_flush_all(struct st_lsm6dsvx_hw *hw) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_mlc_sensor_list); i++) + st_lsm6dsvx_mlc_flush_single(hw, st_lsm6dsvx_mlc_sensor_list[i]); + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsvx_fsm_sensor_list); i++) + st_lsm6dsvx_mlc_flush_single(hw, st_lsm6dsvx_fsm_sensor_list[i]); + + return 0; +} + +static ssize_t st_lsm6dsvx_mlc_info(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsvx_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_lsm6dsvx_mlc_get_version(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "mlc loader Version %s\n", + ST_LSM6DSVX_MLC_LOADER_VERSION); +} + +static ssize_t st_lsm6dsvx_mlc_flush(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsvx_hw *hw = sensor->hw; + int ret; + + ret = st_lsm6dsvx_mlc_flush_all(hw); + memset(hw->mlc_config, 0, sizeof(*hw->mlc_config)); + + return ret < 0 ? ret : size; +} + +static ssize_t +st_lsm6dsvx_mlc_upload_firmware(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err; + + err = request_firmware_nowait(THIS_MODULE, true, + LSM6DSVX_MLC_FIRMWARE_NAME, + dev, GFP_KERNEL, + sensor->hw, + st_lsm6dsvx_mlc_update); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6dsvx_mlc_odr(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsvx_hw *hw = sensor->hw; + + return scnprintf(buf, PAGE_SIZE, "%d\n", + hw->mlc_config->requested_odr); +} + +static IIO_DEVICE_ATTR(config_info, 0444, st_lsm6dsvx_mlc_info, NULL, 0); +static IIO_DEVICE_ATTR(flush_config, 0200, NULL, st_lsm6dsvx_mlc_flush, 0); +static IIO_DEVICE_ATTR(loader_version, 0444, + st_lsm6dsvx_mlc_get_version, NULL, 0); +static IIO_DEVICE_ATTR(load_mlc, 0200, + NULL, st_lsm6dsvx_mlc_upload_firmware, 0); +static IIO_DEVICE_ATTR(odr, 0444, st_lsm6dsvx_mlc_odr, NULL, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsvx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_mlc_event_attribute_group = { + .attrs = st_lsm6dsvx_mlc_event_attributes, +}; + +static const struct iio_info st_lsm6dsvx_mlc_event_info = { + .attrs = &st_lsm6dsvx_mlc_event_attribute_group, + .read_event_config = st_lsm6dsvx_mlc_read_event_config, + .write_event_config = st_lsm6dsvx_mlc_write_event_config, +}; + +static ssize_t st_lsm6dsvx_mlc_x_odr(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsvx_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_lsm6dsvx_mlc_x_odr, NULL, 0); + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_mlc_x_event_attribute_group = { + .attrs = st_lsm6dsvx_mlc_x_event_attributes, +}; +static const struct iio_info st_lsm6dsvx_mlc_x_event_info = { + .attrs = &st_lsm6dsvx_mlc_x_event_attribute_group, + .read_event_config = st_lsm6dsvx_mlc_read_event_config, + .write_event_config = st_lsm6dsvx_mlc_write_event_config, +}; + +static struct iio_dev * +st_lsm6dsvx_mlc_alloc_iio_dev(struct st_lsm6dsvx_hw *hw, + enum st_lsm6dsvx_sensor_id id) +{ + struct st_lsm6dsvx_sensor *sensor; + struct iio_chan_spec *channels; + struct iio_dev *iio_dev; + + /* devm management only for ST_LSM6DSVX_ID_MLC */ + if (id == ST_LSM6DSVX_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_LSM6DSVX_ID_MLC: { + const struct iio_chan_spec st_lsm6dsvx_mlc_channels[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = devm_kzalloc(hw->dev, + sizeof(st_lsm6dsvx_mlc_channels), + GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsvx_mlc_channels, + sizeof(st_lsm6dsvx_mlc_channels)); + + iio_dev->available_scan_masks = + st_lsm6dsvx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_mlc_channels); + iio_dev->info = &st_lsm6dsvx_mlc_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_loader", hw->settings->id.name); + break; + } + case ST_LSM6DSVX_ID_MLC_0: + case ST_LSM6DSVX_ID_MLC_1: + case ST_LSM6DSVX_ID_MLC_2: + case ST_LSM6DSVX_ID_MLC_3: { + const struct iio_chan_spec st_lsm6dsvx_mlc_x_ch[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsvx_mlc_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsvx_mlc_x_ch, + sizeof(st_lsm6dsvx_mlc_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsvx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_mlc_x_ch); + iio_dev->info = &st_lsm6dsvx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_mlc_%d", hw->settings->id.name, + id - ST_LSM6DSVX_ID_MLC_0); + sensor->outreg_addr = ST_LSM6DSVX_REG_MLC1_SRC_ADDR + + id - ST_LSM6DSVX_ID_MLC_0; + sensor->status = ST_LSM6DSVX_MLC_ENABLED; + sensor->odr = hw->mlc_config->requested_odr; + sensor->uodr = 0; + + break; + } + case ST_LSM6DSVX_ID_FSM_0: + case ST_LSM6DSVX_ID_FSM_1: + case ST_LSM6DSVX_ID_FSM_2: + case ST_LSM6DSVX_ID_FSM_3: + case ST_LSM6DSVX_ID_FSM_4: + case ST_LSM6DSVX_ID_FSM_5: + case ST_LSM6DSVX_ID_FSM_6: + case ST_LSM6DSVX_ID_FSM_7: { + const struct iio_chan_spec st_lsm6dsvx_fsm_x_ch[] = { + ST_LSM6DSVX_EVENT_CHANNEL(IIO_ACTIVITY, thr), + }; + + channels = kzalloc(sizeof(st_lsm6dsvx_fsm_x_ch), GFP_KERNEL); + if (!channels) + return NULL; + + memcpy(channels, st_lsm6dsvx_fsm_x_ch, + sizeof(st_lsm6dsvx_fsm_x_ch)); + + iio_dev->available_scan_masks = + st_lsm6dsvx_mlc_available_scan_masks; + iio_dev->channels = channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_fsm_x_ch); + iio_dev->info = &st_lsm6dsvx_mlc_x_event_info; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_fsm_%d", hw->settings->id.name, + id - ST_LSM6DSVX_ID_FSM_0); + sensor->outreg_addr = ST_LSM6DSVX_REG_FSM_OUTS1_ADDR + + id - ST_LSM6DSVX_ID_FSM_0; + sensor->status = ST_LSM6DSVX_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_lsm6dsvx_mlc_check_status(struct st_lsm6dsvx_hw *hw) +{ + struct st_lsm6dsvx_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_LSM6DSVX_MLC_ENABLED) { + err = st_lsm6dsvx_read_locked(hw, + ST_LSM6DSVX_REG_MLC_STATUS_MAINPAGE_ADDR, + (void *)&mlc_status, 1); + if (err) + return err; + + if (mlc_status) { + for (i = 0; i < ST_LSM6DSVX_MLC_MAX_NUMBER; i++) { + id = st_lsm6dsvx_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_lsm6dsvx_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_LSM6DSVX_FSM_ENABLED) { + err = st_lsm6dsvx_read_locked(hw, + ST_LSM6DSVX_REG_FSM_STATUS_MAINPAGE_ADDR, + (void *)&fsm_status, 1); + if (err) + return err; + + if (fsm_status) { + for (i = 0; i < ST_LSM6DSVX_FSM_MAX_NUMBER; i++) { + id = st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_mlc_init_preload(struct st_lsm6dsvx_hw *hw) +{ + hw->preload_mlc = 1; + st_lsm6dsvx_mlc_update(&st_lsm6dsvx_mlc_preload, hw); + + return 0; +} + +int st_lsm6dsvx_mlc_probe(struct st_lsm6dsvx_hw *hw) +{ + hw->iio_devs[ST_LSM6DSVX_ID_MLC] = + st_lsm6dsvx_mlc_alloc_iio_dev(hw, ST_LSM6DSVX_ID_MLC); + if (!hw->iio_devs[ST_LSM6DSVX_ID_MLC]) + return -ENOMEM; + + hw->mlc_config = devm_kzalloc(hw->dev, + sizeof(struct st_lsm6dsvx_mlc_config_t), + GFP_KERNEL); + if (!hw->mlc_config) + return -ENOMEM; + + return 0; +} + +int st_lsm6dsvx_mlc_remove(struct device *dev) +{ + struct st_lsm6dsvx_hw *hw = dev_get_drvdata(dev); + + return st_lsm6dsvx_mlc_flush_all(hw); +} +EXPORT_SYMBOL(st_lsm6dsvx_mlc_remove); diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_preload_mlc.h b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_preload_mlc.h new file mode 100644 index 000000000000..2ebeb8ee9a0d --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_preload_mlc.h @@ -0,0 +1,20 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_LSM6DSVX_PRELOAD_MLC_H +#define ST_LSM6DSVX_PRELOAD_MLC_H + +static const u8 mlcdata[] = { + /* put here MLC/FSM configuration */ +}; + +static struct firmware st_lsm6dsvx_mlc_preload = { + .size = sizeof(mlcdata), + .data = mlcdata +}; + +#endif /* ST_LSM6DSVX_PRELOAD_MLC_H */ diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_qvar.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_qvar.c new file mode 100644 index 000000000000..c155e31ebd88 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_qvar.c @@ -0,0 +1,371 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx qvar sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO +static const u8 qvar_fifo_config[][2] = { + { 0x05, 0x00 }, { 0x17, 0x40 }, { 0x02, 0x11 }, { 0x08, 0xE8 }, + { 0x09, 0x00 }, { 0x09, 0x3C }, { 0x09, 0x96 }, { 0x09, 0x03 }, + { 0x09, 0xB0 }, { 0x09, 0x03 }, { 0x09, 0x02 }, { 0x09, 0x00 }, + { 0x09, 0x14 }, { 0x02, 0x11 }, { 0x08, 0xF2 }, { 0x09, 0xFF }, + { 0x02, 0x11 }, { 0x08, 0xFA }, { 0x09, 0x80 }, { 0x09, 0x03 }, + { 0x09, 0xB2 }, { 0x09, 0x03 }, { 0x09, 0xBE }, { 0x09, 0x03 }, + { 0x02, 0x31 }, { 0x08, 0x80 }, { 0x09, 0xA8 }, { 0x09, 0x00 }, + { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, + { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x3C }, + { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, + { 0x09, 0x3F }, { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x2C }, + { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x1F }, { 0x09, 0x00 }, + { 0x02, 0x31 }, { 0x08, 0xB2 }, { 0x09, 0x00 }, { 0x09, 0x00 }, + { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, + { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, { 0x09, 0x00 }, + { 0x09, 0x00 }, { 0x08, 0xBE }, { 0x09, 0x00 }, { 0x09, 0x00 }, + { 0x09, 0x40 }, { 0x09, 0xE0 }, { 0x17, 0x00 }, { 0x04, 0x00 }, + { 0x05, 0x10 }, { 0x02, 0x01 }, { 0x60, 0x45 }, { 0x45, 0x02 }, +}; +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + +static const struct st_lsm6dsvx_odr_table_entry +st_lsm6dsvx_qvar_odr_table = { + .size = 1, + .odr_avl[0] = { 240, 0, 0x00, 0x00 }, +}; + +static const struct iio_chan_spec st_lsm6dsvx_qvar_channels[] = { + { + .type = IIO_ALTVOLTAGE, + .address = ST_LSM6DSVX_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_lsm6dsvx_qvar_init(struct st_lsm6dsvx_hw *hw) +{ + +#ifdef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + uint8_t i; + int err; + + mutex_lock(&hw->page_lock); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 1); + for (i = 0; i < ARRAY_SIZE(qvar_fifo_config); i++) { + err = regmap_write(hw->regmap, qvar_fifo_config[i][0], + qvar_fifo_config[i][1]); + if (err < 0) { + st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + mutex_unlock(&hw->page_lock); + dev_err(hw->dev, "failed to configure qvar\n"); + + return err; + } + } + st_lsm6dsvx_set_page_access(hw, + ST_LSM6DSVX_EMB_FUNC_REG_ACCESS_MASK, 0); + mutex_unlock(&hw->page_lock); +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + /* impedance selection */ + return st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_CTRL7_ADDR, + ST_LSM6DSVX_AH_QVAR_C_ZIN_MASK, 3); +} + +static ssize_t +st_lsm6dsvx_sysfs_qvar_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int len = 0; + int i; + + for (i = 0; i < st_lsm6dsvx_qvar_odr_table.size; i++) { + if (!st_lsm6dsvx_qvar_odr_table.odr_avl[i].hz) + continue; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_lsm6dsvx_qvar_odr_table.odr_avl[i].hz, + st_lsm6dsvx_qvar_odr_table.odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsvx_sysfs_qvar_sampling_freq_avail); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsvx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsvx_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_lsm6dsvx_qvar_attribute_group = { + .attrs = st_lsm6dsvx_qvar_attributes, +}; + +static const struct iio_info st_lsm6dsvx_qvar_info = { + .attrs = &st_lsm6dsvx_qvar_attribute_group, +}; + +static const unsigned long st_lsm6dsvx_qvar_available_scan_masks[] = { + BIT(0), 0x0 +}; + +static int +_st_lsm6dsvx_qvar_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + u16 odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsvx_sensor_set_enable(sensor, odr); + if (err < 0) + return err; + +#ifndef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + if (enable) { + int64_t newTime; + + newTime = 1000000000 / odr; + sensor->oldktime = ktime_set(0, newTime); + hrtimer_start(&sensor->hr_timer, sensor->oldktime, + HRTIMER_MODE_REL); + } else { + cancel_work_sync(&sensor->iio_work); + hrtimer_cancel(&sensor->hr_timer); + } +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + return st_lsm6dsvx_write_with_mask(sensor->hw, + ST_LSM6DSVX_REG_CTRL7_ADDR, + ST_LSM6DSVX_AH_QVAR_EN_MASK, + enable ? 1 : 0); +} + +int +st_lsm6dsvx_qvar_sensor_set_enable(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + int err; + + err = _st_lsm6dsvx_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; +} + +#ifndef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO +static inline void st_lsm6dsvx_flush_works(struct st_lsm6dsvx_hw *hw) +{ + flush_workqueue(hw->qvar_workqueue); +} + +static int st_lsm6dsvx_allocate_workqueue(struct st_lsm6dsvx_hw *hw) +{ + if (!hw->qvar_workqueue) + hw->qvar_workqueue = + create_workqueue(hw->iio_devs[ST_LSM6DSVX_ID_QVAR]->name); + + if (!hw->qvar_workqueue) + return -ENOMEM; + + return 0; +} + +static enum hrtimer_restart +st_lsm6dsvx_qvar_poll_function_read(struct hrtimer *timer) +{ + struct st_lsm6dsvx_sensor *sensor; + + sensor = container_of((struct hrtimer *)timer, + struct st_lsm6dsvx_sensor, hr_timer); + + sensor->timestamp = + iio_get_time_ns(sensor->hw->iio_devs[ST_LSM6DSVX_ID_QVAR]); + queue_work(sensor->hw->qvar_workqueue, &sensor->iio_work); + + return HRTIMER_NORESTART; +} + +static void +st_lsm6dsvx_report_1axes_event(struct st_lsm6dsvx_sensor *sensor, + u8 *tmp, int64_t timestamp) +{ + struct iio_dev *iio_dev = sensor->hw->iio_devs[sensor->id]; + u8 iio_buf[ALIGN(ST_LSM6DSVX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)]; + + memcpy(iio_buf, tmp, ST_LSM6DSVX_SAMPLE_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, timestamp); +} + +static int +st_lsm6dsvx_get_qvar_poll_data(struct st_lsm6dsvx_sensor *sensor, + u8 *data) +{ + return st_lsm6dsvx_read_locked(sensor->hw, + ST_LSM6DSVX_REG_OUT_QVAR_ADDR, data, 2); +} + +static void +st_lsm6dsvx_qvar_poll_function_work(struct work_struct *iio_work) +{ + struct st_lsm6dsvx_sensor *sensor; + u8 data[2]; + int err; + + sensor = container_of((struct work_struct *)iio_work, + struct st_lsm6dsvx_sensor, iio_work); + + hrtimer_start(&sensor->hr_timer, sensor->oldktime, HRTIMER_MODE_REL); + err = st_lsm6dsvx_get_qvar_poll_data(sensor, data); + if (err < 0) + return; + + st_lsm6dsvx_report_1axes_event(sensor, data, sensor->timestamp); +} + +static int st_lsm6dsvx_qvar_preenable(struct iio_dev *iio_dev) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsvx_qvar_sensor_set_enable(sensor, true); +} + +static int st_lsm6dsvx_qvar_postdisable(struct iio_dev *iio_dev) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsvx_qvar_sensor_set_enable(sensor, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsvx_qvar_ops = { + .preenable = st_lsm6dsvx_qvar_preenable, + .postdisable = st_lsm6dsvx_qvar_postdisable, +}; + +static int st_lsm6dsvx_qvar_buffer(struct st_lsm6dsvx_hw *hw) +{ + struct iio_buffer *buffer; + + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[ST_LSM6DSVX_ID_QVAR], buffer); + hw->iio_devs[ST_LSM6DSVX_ID_QVAR]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[ST_LSM6DSVX_ID_QVAR]->setup_ops = &st_lsm6dsvx_qvar_ops; + + return st_lsm6dsvx_allocate_workqueue(hw); +} +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + +static struct iio_dev * +st_lsm6dsvx_alloc_qvar_iiodev(struct st_lsm6dsvx_hw *hw) +{ + struct st_lsm6dsvx_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_LSM6DSVX_ID_QVAR; + sensor->hw = hw; + + iio_dev->channels = st_lsm6dsvx_qvar_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsvx_qvar_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "%s_qvar", hw->settings->id.name); + iio_dev->info = &st_lsm6dsvx_qvar_info; + iio_dev->available_scan_masks = st_lsm6dsvx_qvar_available_scan_masks; + iio_dev->name = sensor->name; + + sensor->odr = st_lsm6dsvx_qvar_odr_table.odr_avl[0].hz; + sensor->uodr = st_lsm6dsvx_qvar_odr_table.odr_avl[0].uhz; + sensor->gain = 1; + sensor->watermark = 1; + +#ifndef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + /* configure hrtimer */ + hrtimer_init(&sensor->hr_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + sensor->hr_timer.function = &st_lsm6dsvx_qvar_poll_function_read; + + sensor->oldktime = ktime_set(0, 1000000000 / sensor->odr); + INIT_WORK(&sensor->iio_work, st_lsm6dsvx_qvar_poll_function_work); +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + return iio_dev; +} + + +int st_lsm6dsvx_qvar_probe(struct st_lsm6dsvx_hw *hw) +{ + int err; + + hw->iio_devs[ST_LSM6DSVX_ID_QVAR] = st_lsm6dsvx_alloc_qvar_iiodev(hw); + if (!hw->iio_devs[ST_LSM6DSVX_ID_QVAR]) + return -ENOMEM; + +#ifndef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + /* if qvar not in FIFO the st_lsm6dsvx_qvar_ops is local */ + err = st_lsm6dsvx_qvar_buffer(hw); + if (err) + return err; +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + err = st_lsm6dsvx_qvar_init(hw); + + return err < 0 ? err : 0; +} + +int st_lsm6dsvx_qvar_remove(struct device *dev) +{ + +#ifndef CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO + struct st_lsm6dsvx_hw *hw = dev_get_drvdata(dev); + + st_lsm6dsvx_flush_works(hw); + destroy_workqueue(hw->qvar_workqueue); + hw->qvar_workqueue = NULL; +#endif /* CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO */ + + return 0; +} diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_shub.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_shub.c new file mode 100644 index 000000000000..e91f6a92ecbf --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_shub.c @@ -0,0 +1,976 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx sensor hub library driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +#define ST_LSM6DSVX_MAX_SLV_NUM 2 + +/** + * @struct st_lsm6dsvx_ext_pwr + * @brief External device Power Management description + * reg: Generic sensor register description. + * off_val: Value to write into register to power off external sensor. + * on_val: Value to write into register for power on external sensor. + */ +struct st_lsm6dsvx_ext_pwr { + struct st_lsm6dsvx_reg reg; + u8 off_val; + u8 on_val; +}; + +/** + * @struct st_lsm6dsvx_ext_dev_settings + * @brief External sensor descriptor entry + * i2c_addr: External I2C device address (max two). + * wai_addr: Device ID address. + * wai_val: Device ID value. + * odr_table: ODR sensor table. + * fs_table: Full scale table. + * temp_comp_reg: Temperature compensation registers. + * pwr_table: External device Power Management description. + * off_canc_reg: Offset cancellation registers. + * bdu_reg: Block Data Update registers. + * ext_available_scan_masks: IIO device scan mask. + * ext_channels:IIO device channel specifications. + * ext_chan_depth: Max number of IIO device channel specifications. + * data_len: Sensor output data len. + */ +struct st_lsm6dsvx_ext_dev_settings { + u8 i2c_addr[2]; + u8 wai_addr; + u8 wai_val; + struct st_lsm6dsvx_odr_table_entry odr_table; + struct st_lsm6dsvx_fs_table_entry fs_table; + struct st_lsm6dsvx_reg temp_comp_reg; + struct st_lsm6dsvx_ext_pwr pwr_table; + struct st_lsm6dsvx_reg off_canc_reg; + struct st_lsm6dsvx_reg bdu_reg; + unsigned long ext_available_scan_masks[2]; + const struct iio_chan_spec ext_channels[5]; + u8 ext_chan_depth; + u8 data_len; +}; + +static const struct st_lsm6dsvx_ext_dev_settings st_lsm6dsvx_ext_dev_table[] = { + /* LIS2MDL */ + { + .i2c_addr = { 0x1e }, + .wai_addr = 0x4f, + .wai_val = 0x40, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + /* + * added 5Hz for CTS coverage, reg value is the + * same for 5 and 10 Hz + * Hz DC REG BATCH + */ + .odr_avl[0] = { 5, 1, 0x0, 0x0 }, + .odr_avl[1] = { 10, 0, 0x0, 0x0 }, + .odr_avl[2] = { 20, 0, 0x1, 0x0 }, + .odr_avl[3] = { 50, 0, 0x2, 0x0 }, + .odr_avl[4] = { 100, 0, 0x3, 0x0 }, + }, + .fs_table = { + .size = 1, + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + }, + .temp_comp_reg = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc_reg = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu_reg = { + .addr = 0x62, + .mask = BIT(4), + }, + .ext_available_scan_masks = { 0x7, 0x0 }, + .ext_channels[0] = ST_LSM6DSVX_DATA_CHANNEL(IIO_MAGN, 0x68, + 1, IIO_MOD_X, 0, + 16, 16, 's', NULL), + .ext_channels[1] = ST_LSM6DSVX_DATA_CHANNEL(IIO_MAGN, 0x6a, + 1, IIO_MOD_Y, 1, + 16, 16, 's', NULL), + .ext_channels[2] = ST_LSM6DSVX_DATA_CHANNEL(IIO_MAGN, 0x6c, + 1, IIO_MOD_Z, 2, + 16, 16, 's', NULL), + .ext_channels[3] = ST_LSM6DSVX_EVENT_CHANNEL(IIO_MAGN, flush), + .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3), + .ext_chan_depth = 5, + .data_len = 6, + }, + /* LPS22HH */ + { + .i2c_addr = { 0x5c, 0x5d }, + .wai_addr = 0x0f, + .wai_val = 0xb3, + .odr_table = { + .size = 5, + .reg = { + .addr = 0x10, + .mask = GENMASK(6, 4), + }, + .odr_avl[0] = { 1, 10, 0x1 }, + .odr_avl[1] = { 10, 0, 0x2 }, + .odr_avl[2] = { 25, 0, 0x3 }, + .odr_avl[3] = { 50, 0, 0x4 }, + .odr_avl[4] = { 100, 0, 0x6 }, + }, + .fs_table = { + .size = 1, + /* hPa miscro scale */ + .fs_avl[0] = { + .gain = 1000000UL/4096UL, + .val = 0x0, + }, + }, + .bdu_reg = { + .addr = 0x10, + .mask = BIT(1), + }, + .ext_available_scan_masks = { 0x1, 0x0 }, + .ext_channels[0] = ST_LSM6DSVX_DATA_CHANNEL(IIO_PRESSURE, 0x28, + 0, IIO_NO_MOD, 0, + 24, 32, 'u', NULL), + .ext_channels[1] = ST_LSM6DSVX_EVENT_CHANNEL(IIO_PRESSURE, + flush), + .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1), + .ext_chan_depth = 3, + .data_len = 3, + }, +}; + +/** + * Wait write trigger [SHUB] + * + * In write on external device register, each operation is triggered + * by accel/gyro data ready, this means that wait time depends on ODR + * plus i2c time + * NOTE: Be sure to enable Acc or Gyro before this operation + * + * @param hw: ST IMU MEMS hw instance. + */ +static inline void st_lsm6dsvx_shub_wait_complete(struct st_lsm6dsvx_hw *hw) +{ + struct st_lsm6dsvx_sensor *sensor; + u16 odr; + + sensor = iio_priv(hw->iio_devs[ST_LSM6DSVX_ID_ACC]); + /* check if acc is enabled */ + odr = (hw->enable_mask & BIT(ST_LSM6DSVX_ID_ACC)) ? sensor->odr : 15; + msleep((2000U / odr) + 1); +} + +/** + * Read from sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsvx_shub_read_reg(struct st_lsm6dsvx_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 1); + if (err < 0) + goto out; + + err = regmap_bulk_read(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 0); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Write to sensor hub bank register [SHUB] + * + * NOTE: uses page_lock + * + * @param hw: ST IMU MEMS hw instance. + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsvx_shub_write_reg(struct st_lsm6dsvx_hw *hw, + u8 addr, u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 1); + if (err < 0) + goto out; + + err = regmap_bulk_write(hw->regmap, (unsigned int)addr, + (unsigned int *)data, len); + + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 0); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Enable sensor hub interface [SHUB] + * + * NOTE: uses page_lock + * + * @param sensor: ST IMU sensor instance + * @param enable: Master Enable/Disable. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_lsm6dsvx_shub_master_enable(struct st_lsm6dsvx_sensor *sensor, bool enable) +{ + struct st_lsm6dsvx_hw *hw = sensor->hw; + int err; + + /* enable acc sensor as trigger */ + err = st_lsm6dsvx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 1); + if (err < 0) + goto out; + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_MASTER_CONFIG_ADDR, + ST_LSM6DSVX_MASTER_ON_MASK, 1); + + err = __st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_SLV0_CONFIG_ADDR, + ST_LSM6DSVX_SHUB_ODR_MASK, + enable == 1 ? ST_LSM6DSVX_REG_SHUB_ODR_120HZ_VAL : 0); + + st_lsm6dsvx_set_page_access(hw, ST_LSM6DSVX_SHUB_REG_ACCESS_MASK, 0); + +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/** + * Read sensor data register from shub interface + * + * NOTE: use SLV3 i2c slave for one-shot read operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsvx_shub_read(struct st_lsm6dsvx_sensor *sensor, + u8 addr, u8 *data, int len) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 out_addr = ST_LSM6DSVX_REG_SENSOR_HUB_1_ADDR + hw->ext_data_len; + u8 config[3]; + int err; + + config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[1] = addr; + config[2] = len & 0x7; + + err = st_lsm6dsvx_shub_write_reg(hw, ST_LSM6DSVX_REG_SLV3_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsvx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsvx_shub_wait_complete(hw); + + err = st_lsm6dsvx_shub_read_reg(hw, out_addr, data, len & 0x7); + + st_lsm6dsvx_shub_master_enable(sensor, false); + + memset(config, 0, sizeof(config)); + return st_lsm6dsvx_shub_write_reg(hw, ST_LSM6DSVX_REG_SLV3_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface + * + * NOTE: use SLV0 i2c slave for write operation + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param data: Data buffer. + * @param len: Data read len. + * @return 0 if OK, < 0 if ERROR + */ +static int st_lsm6dsvx_shub_write(struct st_lsm6dsvx_sensor *sensor, + u8 addr, u8 *data, int len) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 mconfig = ST_LSM6DSVX_WRITE_ONCE_MASK | 3; + u8 config[3] = { 0, 0, 0x80 }; + int err, i; + + /* AuxSens = 3 + wr once */ + err = st_lsm6dsvx_shub_write_reg(hw, + ST_LSM6DSVX_REG_MASTER_CONFIG_ADDR, + &mconfig, sizeof(mconfig)); + if (err < 0) + return err; + + config[0] = ext_info->ext_dev_i2c_addr << 1; + for (i = 0; i < len; i++) { + config[1] = addr + i; + + err = st_lsm6dsvx_shub_write_reg(hw, + ST_LSM6DSVX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsvx_shub_write_reg(hw, + ST_LSM6DSVX_REG_DATAWRITE_SLV0_ADDR, + &data[i], 1); + if (err < 0) + return err; + + err = st_lsm6dsvx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsvx_shub_wait_complete(hw); + + st_lsm6dsvx_shub_master_enable(sensor, false); + } + + return st_lsm6dsvx_shub_write_reg(hw, ST_LSM6DSVX_REG_SLV0_ADDR, + config, sizeof(config)); +} + +/** + * Write sensor data register from shub interface using register bitmask + * + * @param sensor: ST IMU sensor instance + * @param addr: Remote address register. + * @param mask: Register bitmask. + * @param val: Data buffer. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_lsm6dsvx_shub_write_with_mask(struct st_lsm6dsvx_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_lsm6dsvx_shub_read(sensor, addr, &data, sizeof(data)); + if (err < 0) + return err; + + data = ((data & ~mask) | (val << __ffs(mask) & mask)); + + return st_lsm6dsvx_shub_write(sensor, addr, &data, sizeof(data)); +} + +/** + * Configure external sensor connected on master I2C interface + * + * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable/Disable sensor. + * @return 0 if OK, < 0 if ERROR + */ +static int +st_lsm6dsvx_shub_config_channels(struct st_lsm6dsvx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info; + struct st_lsm6dsvx_hw *hw = sensor->hw; + struct st_lsm6dsvx_sensor *cur_sensor; + u8 config[6] = {}, enable_mask; + int i, j = 0; + + enable_mask = enable ? hw->enable_mask | BIT(sensor->id) + : hw->enable_mask & ~BIT(sensor->id); + + for (i = ST_LSM6DSVX_ID_EXT0; i <= ST_LSM6DSVX_ID_EXT1; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT(cur_sensor->id))) + continue; + + ext_info = &cur_sensor->ext_dev_info; + config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1; + config[j + 1] = ext_info->ext_dev_settings->ext_channels[0].address; + config[j + 2] = ST_LSM6DSVX_REG_BATCH_EXT_SENS_EN_MASK | + (ext_info->ext_dev_settings->data_len & + ST_LSM6DSVX_REG_SLAVE_NUMOP_MASK); + j += 3; + } + + return st_lsm6dsvx_shub_write_reg(hw, ST_LSM6DSVX_REG_SLV1_ADDR, + config, sizeof(config)); +} + +/** + * Get a valid ODR [SHUB] + * + * Check a valid ODR closest to the passed value + * + * @param sensor: SST IMU sensor instance. + * @param odr: ODR value (in Hz). + * @param val: ODR register value data pointer. + * @return 0 if OK, negative value for ERROR + */ +static int +st_lsm6dsvx_shub_get_odr_val(struct st_lsm6dsvx_sensor *sensor, + u16 odr, u8 *val) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i; + + for (i = 0; i < ext_info->ext_dev_settings->odr_table.size; i++) + if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr) + break; + + if (i == ext_info->ext_dev_settings->odr_table.size) + return -EINVAL; + + *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val; + + /* set decimator for ODR < 10 Hz */ + sensor->decimator = + ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz; + sensor->dec_counter = 0; + + return 0; +} + +/** + * Set new ODR to sensor [SHUB] + * + * Set a valid ODR closest to the passed value + * + * @param sensor: ST IMU sensor instance + * @param odr: ODR value (in Hz). + * @return 0 if OK, negative value for ERROR + */ +static int st_lsm6dsvx_shub_set_odr(struct st_lsm6dsvx_sensor *sensor, u16 odr) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + struct st_lsm6dsvx_hw *hw = sensor->hw; + u8 odr_val; + int err; + + err = st_lsm6dsvx_shub_get_odr_val(sensor, odr, &odr_val); + if (err < 0) + return err; + + if (sensor->odr == odr && (hw->enable_mask & BIT(sensor->id))) + return 0; + + return st_lsm6dsvx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, + odr_val); +} + +/** + * Enable or Disable sensor [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param enable: Enable or disable the sensor [true,false]. + * @return 0 if OK, negative value for ERROR + */ +int st_lsm6dsvx_shub_set_enable(struct st_lsm6dsvx_sensor *sensor, bool enable) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err; + + err = st_lsm6dsvx_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + if (enable) { + err = st_lsm6dsvx_shub_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + } else { + err = st_lsm6dsvx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->odr_table.reg.addr, + ext_info->ext_dev_settings->odr_table.reg.mask, 0); + if (err < 0) + return err; + } + + if (ext_info->ext_dev_settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? ext_info->ext_dev_settings->pwr_table.on_val + : ext_info->ext_dev_settings->pwr_table.off_val; + err = st_lsm6dsvx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->pwr_table.reg.addr, + ext_info->ext_dev_settings->pwr_table.reg.mask, + val); + if (err < 0) + return err; + } + + return st_lsm6dsvx_shub_master_enable(sensor, enable); +} + +static inline u32 st_lsm6dsvx_get_unaligned_le24(const u8 *p) +{ + return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; +} + +/** + * Single sensor read operation [SHUB] + * + * @param sensor: ST IMU sensor instance + * @param ch: IIO Channel. + * @param val: Output data register value. + * @return IIO_VAL_INT if OK, negative value for ERROR + */ +static int +st_lsm6dsvx_shub_read_oneshot(struct st_lsm6dsvx_sensor *sensor, + struct iio_chan_spec const *ch, int *val) +{ + int err, delay, len = ch->scan_type.realbits >> 3; + u8 data[4]; + + err = st_lsm6dsvx_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = st_lsm6dsvx_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + st_lsm6dsvx_shub_set_enable(sensor, false); + + switch (len) { + case 3: + *val = (s32)st_lsm6dsvx_get_unaligned_le24(data); + break; + case 2: + *val = (s16)get_unaligned_le16(data); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +/** + * Read Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param ch: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_lsm6dsvx_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsvx_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_lsm6dsvx_shub_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/** + * Write Sensor data configuration [SHUB] + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param val: Data Buffer (MSB). + * @param val2: Data Buffer (LSB). + * @param mask: Data Mask. + * @return 0 if OK, -EINVAL value for ERROR + */ +static int st_lsm6dsvx_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_lsm6dsvx_shub_get_odr_val(sensor, val, &data); + if (!err) + sensor->odr = val; + break; + } + case IIO_CHAN_INFO_SCALE: + err = 0; + break; + default: + err = -EINVAL; + break; + } + + iio_device_release_direct_mode(iio_dev); + + return err; +} + +/** + * Get a list of available sensor ODR [SHUB] + * + * List of available ODR returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_lsm6dsvx_sysfs_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ST_LSM6DSVX_ODR_LIST_SIZE; i++) { + u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, + "%d ", val); + } + buf[len - 1] = '\n'; + + return len; +} + +/** + * Get a list of available sensor Full Scale [SHUB] + * + * List of available Full Scale returned separated by commas + * + * @param dev: IIO Device. + * @param attr: IIO Channel attribute. + * @param buf: User buffer. + * @return buffer len + */ +static ssize_t +st_lsm6dsvx_sysfs_shub_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsvx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int i, len = 0; + + for (i = 0; i < ext_info->ext_dev_settings->fs_table.size; i++) { + u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain; + + if (val > 0) + len += scnprintf(buf + len, PAGE_SIZE - len, + "0.%06u ", val); + } + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsvx_sysfs_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_ext_scale_available, 0444, + st_lsm6dsvx_sysfs_shub_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lsm6dsvx_get_max_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsvx_flush_fifo, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsvx_get_watermark, + st_lsm6dsvx_set_watermark, 0); +static IIO_DEVICE_ATTR(module_id, 0444, st_lsm6dsvx_get_module_id, NULL, 0); + +static struct attribute *st_lsm6dsvx_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_ext_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_lsm6dsvx_ext_attribute_group = { + .attrs = st_lsm6dsvx_ext_attributes, +}; + +static const struct iio_info st_lsm6dsvx_ext_info = { + .attrs = &st_lsm6dsvx_ext_attribute_group, + .read_raw = st_lsm6dsvx_shub_read_raw, + .write_raw = st_lsm6dsvx_shub_write_raw, +}; + +/** + * Allocate IIO device [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @param ext_settings: xternal sensor descritor entry. + * @param id: Sensor Identifier. + * @param i2c_addr: external I2C address on master bus. + * @return struct iio_dev *, NULL if ERROR + */ +static struct iio_dev * +st_lsm6dsvx_shub_alloc_iio_dev(struct st_lsm6dsvx_hw *hw, + const struct st_lsm6dsvx_ext_dev_settings *ext_settings, + enum st_lsm6dsvx_sensor_id id, u8 i2c_addr) +{ + struct st_lsm6dsvx_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; + iio_dev->available_scan_masks = ext_settings->ext_available_scan_masks; + iio_dev->info = &st_lsm6dsvx_ext_info; + iio_dev->channels = ext_settings->ext_channels; + iio_dev->num_channels = ext_settings->ext_chan_depth; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = ext_settings->odr_table.odr_avl[0].hz; + sensor->gain = ext_settings->fs_table.fs_avl[0].gain; + sensor->max_watermark = ST_LSM6DSVX_MAX_FIFO_DEPTH; + sensor->watermark = 1; + sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr; + sensor->ext_dev_info.ext_dev_settings = ext_settings; + sensor->decimator = 0; + sensor->dec_counter = 0; + + switch (iio_dev->channels[0].type) { + case IIO_MAGN: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_magn", hw->settings->id.name); + break; + case IIO_PRESSURE: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", hw->settings->id.name); + break; + default: + scnprintf(sensor->name, sizeof(sensor->name), + "%s_ext", hw->settings->id.name); + break; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static int +st_lsm6dsvx_shub_init_remote_sensor(struct st_lsm6dsvx_sensor *sensor) +{ + struct st_lsm6dsvx_ext_dev_info *ext_info = &sensor->ext_dev_info; + int err = 0; + + if (ext_info->ext_dev_settings->bdu_reg.addr) + err = st_lsm6dsvx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->bdu_reg.addr, + ext_info->ext_dev_settings->bdu_reg.mask, 1); + + if (ext_info->ext_dev_settings->temp_comp_reg.addr) + err = st_lsm6dsvx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->temp_comp_reg.addr, + ext_info->ext_dev_settings->temp_comp_reg.mask, 1); + + if (ext_info->ext_dev_settings->off_canc_reg.addr) + err = st_lsm6dsvx_shub_write_with_mask(sensor, + ext_info->ext_dev_settings->off_canc_reg.addr, + ext_info->ext_dev_settings->off_canc_reg.mask, 1); + + return err; +} + +/** + * Probe device function [SHUB] + * + * @param hw: ST IMU MEMS hw instance. + * @return 0 if OK, negative for ERROR + */ +int st_lsm6dsvx_shub_probe(struct st_lsm6dsvx_hw *hw) +{ + const struct st_lsm6dsvx_ext_dev_settings *settings; + struct st_lsm6dsvx_sensor *acc_sensor, *sensor; + u8 config[3], data, num_ext_dev = 0; + enum st_lsm6dsvx_sensor_id id; + int err, i = 0, j; + struct device_node *np = hw->dev->of_node; + + if (np && of_property_read_bool(np, "drive-pullup-shub")) { + dev_info(hw->dev, "enabling pull up on i2c master\n"); + + err = st_lsm6dsvx_write_with_mask(hw, + ST_LSM6DSVX_REG_IF_CFG_ADDR, + ST_LSM6DSVX_SHUB_PU_EN_MASK, + 1); + if (err < 0) + return err; + } + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSVX_ID_ACC]); + while (i < ARRAY_SIZE(st_lsm6dsvx_ext_dev_table) && + num_ext_dev < ST_LSM6DSVX_MAX_SLV_NUM) { + settings = &st_lsm6dsvx_ext_dev_table[i]; + for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) { + if (!settings->i2c_addr[j]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[j] << 1) | 1; + config[1] = settings->wai_addr; + config[2] = 0x1; + + err = st_lsm6dsvx_shub_write_reg(hw, + ST_LSM6DSVX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsvx_shub_master_enable(acc_sensor, true); + if (err < 0) + return err; + + st_lsm6dsvx_shub_wait_complete(hw); + + err = st_lsm6dsvx_shub_read_reg(hw, + ST_LSM6DSVX_REG_SENSOR_HUB_1_ADDR, + &data, sizeof(data)); + + st_lsm6dsvx_shub_master_enable(acc_sensor, false); + + if (err < 0) + return err; + + if (data != settings->wai_val) + continue; + + id = ST_LSM6DSVX_ID_EXT0 + num_ext_dev; + hw->iio_devs[id] = st_lsm6dsvx_shub_alloc_iio_dev(hw, + settings, id, + settings->i2c_addr[j]); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_lsm6dsvx_shub_init_remote_sensor(sensor); + if (err < 0) + return err; + + num_ext_dev++; + hw->ext_data_len += settings->data_len; + break; + } + + i++; + } + + if (!num_ext_dev) + return 0; + + memset(config, 0, sizeof(config)); + err = st_lsm6dsvx_shub_write_reg(hw, ST_LSM6DSVX_REG_SLV0_ADDR, + config, sizeof(config)); + if (err < 0) + return err; + + /* AuxSens = 3 + wr once */ + data = ST_LSM6DSVX_WRITE_ONCE_MASK | 3; + return st_lsm6dsvx_shub_write_reg(hw, + ST_LSM6DSVX_REG_MASTER_CONFIG_ADDR, + &data, sizeof(data)); +} diff --git a/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_spi.c b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_spi.c new file mode 100644 index 000000000000..80eff4bf9da3 --- /dev/null +++ b/drivers/iio/stm/imu/st_lsm6dsvx/st_lsm6dsvx_spi.c @@ -0,0 +1,78 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsvx spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_lsm6dsvx.h" + +static const struct regmap_config st_lsm6dsvx_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsvx_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_lsm6dsvx_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_lsm6dsvx_probe(&spi->dev, spi->irq, hw_id, regmap); +} + +static int st_lsm6dsvx_spi_remove(struct spi_device *spi) +{ + return st_lsm6dsvx_remove(&spi->dev); +} + +static const struct of_device_id st_lsm6dsvx_spi_of_match[] = { + { + .compatible = "st," ST_LSM6DSV_DEV_NAME, + .data = (void *)ST_LSM6DSV_ID, + }, + { + .compatible = "st," ST_LSM6DSV16X_DEV_NAME, + .data = (void *)ST_LSM6DSVX_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsvx_spi_of_match); + +static const struct spi_device_id st_lsm6dsvx_spi_id_table[] = { + { ST_LSM6DSV_DEV_NAME, ST_LSM6DSV_ID }, + { ST_LSM6DSV16X_DEV_NAME, ST_LSM6DSVX_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsvx_spi_id_table); + +static struct spi_driver st_lsm6dsvx_driver = { + .driver = { + .name = "st_lsm6dsvx_spi", + .pm = &st_lsm6dsvx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsvx_spi_of_match), + }, + .probe = st_lsm6dsvx_spi_probe, + .remove = st_lsm6dsvx_spi_remove, + .id_table = st_lsm6dsvx_spi_id_table, +}; +module_spi_driver(st_lsm6dsvx_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsvx spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/Kconfig b/drivers/iio/stm/magnetometer/Kconfig new file mode 100644 index 000000000000..7bd17f9b573b --- /dev/null +++ b/drivers/iio/stm/magnetometer/Kconfig @@ -0,0 +1,57 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Magnetometer sensors +# +# When adding new entries keep the list in alphabetical order + +menu "Magnetometer sensors" + +config ST_MAG3D_IIO + tristate "STMicroelectronics LIS3MDL/LSM9DS1 mag sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select ST_MAG3D_I2C_IIO if (I2C) + select ST_MAG3D_SPI_IIO if (SPI) + help + Say yes here to build support for STMicroelectronics magnetometers: + LIS3MDL, LSM9DS1 mag sensor + + To compile this driver as a module, choose M here. The module + will be called st_mag3d + +config ST_MAG3D_I2C_IIO + tristate + depends on ST_MAG3D_IIO + depends on I2C + +config ST_MAG3D_SPI_IIO + tristate + depends on ST_MAG3D_IIO + depends on SPI + +config ST_MAG40_IIO + tristate "STMicroelectronics LIS2MDL/LSM303AH/LSM303AGR/ISM303DAC/IIS2MDC sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select ST_MAG40_I2C_IIO if (I2C) + select ST_MAG40_SPI_IIO if (SPI) + help + Say yes here to build support for STMicroelectronics magnetometers: + LIS2MDL, LSM303AH, LSM303AGR, ISM303DAC, IIS2MDC. + + To compile this driver as a module, choose M here. The module + will be called st_mag40. + +config ST_MAG40_I2C_IIO + tristate + depends on ST_MAG40_IIO + depends on I2C + +config ST_MAG40_SPI_IIO + tristate + depends on ST_MAG40_IIO + depends on SPI + +endmenu diff --git a/drivers/iio/stm/magnetometer/Makefile b/drivers/iio/stm/magnetometer/Makefile new file mode 100644 index 000000000000..c4de28605bbf --- /dev/null +++ b/drivers/iio/stm/magnetometer/Makefile @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for industrial I/O Magnetometer sensor drivers +# + +# When adding new entries keep the list in alphabetical order +st_mag3d-y += st_mag3d_core.o st_mag3d_buffer.o +obj-$(CONFIG_ST_MAG3D_IIO) += st_mag3d.o +obj-$(CONFIG_ST_MAG3D_I2C_IIO) += st_mag3d_i2c.o +obj-$(CONFIG_ST_MAG3D_SPI_IIO) += st_mag3d_spi.o + +st_mag40-y += st_mag40_buffer.o st_mag40_core.o +obj-$(CONFIG_ST_MAG40_IIO) += st_mag40.o +obj-$(CONFIG_ST_MAG40_I2C_IIO) += st_mag40_i2c.o +obj-$(CONFIG_ST_MAG40_SPI_IIO) += st_mag40_spi.o diff --git a/drivers/iio/stm/magnetometer/st_mag3d.h b/drivers/iio/stm/magnetometer/st_mag3d.h new file mode 100644 index 000000000000..4cf4e57698a5 --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag3d.h @@ -0,0 +1,72 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_mag3d driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef __ST_MAG3D_H +#define __ST_MAG3D_H + +#include + +#define LIS3MDL_DEV_NAME "lis3mdl_magn" +#define LSM9DS1_DEV_NAME "lsm9ds1_magn" + +#define ST_MAG3D_TX_MAX_LENGTH 16 +#define ST_MAG3D_RX_MAX_LENGTH 16 + +#define ST_MAG3D_SAMPLE_SIZE 6 + +#define ST_MAG3D_EWMA_DIV 128 +#define ST_MAG3D_EWMA_WEIGHT 96 + +struct iio_dev; + +struct st_mag3d_transfer_buffer { + u8 rx_buf[ST_MAG3D_RX_MAX_LENGTH]; + u8 tx_buf[ST_MAG3D_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_mag3d_transfer_function { + int (*write)(struct device *dev, u8 addr, int len, u8 *data); + int (*read)(struct device *dev, u8 addr, int len, u8 *data); +}; + +struct st_mag3d_hw { + struct device *dev; + struct mutex lock; + u8 buffer[ALIGN(ST_MAG3D_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)]; + u16 odr; + u16 gain; + u8 stodis; + + s64 timestamp; + s64 delta_ts; + s64 mag_ts; + + struct iio_trigger *trig; + int irq; + + const struct st_mag3d_transfer_function *tf; + struct st_mag3d_transfer_buffer tb; + struct iio_dev *iio_dev; +}; + +static inline s64 st_mag3d_get_time_ns(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +int st_mag3d_probe(struct device *dev, int irq, const char *name, + const struct st_mag3d_transfer_function *tf_ops); +void st_mag3d_remove(struct iio_dev *iio_dev); +int st_mag3d_allocate_buffer(struct iio_dev *iio_dev); +void st_mag3d_deallocate_buffer(struct iio_dev *iio_dev); +int st_mag3d_allocate_trigger(struct iio_dev *iio_dev); +void st_mag3d_deallocate_trigger(struct iio_dev *iio_dev); +int st_mag3d_enable_sensor(struct st_mag3d_hw *hw, bool enable); + +#endif /* __ST_MAG3D_H */ diff --git a/drivers/iio/stm/magnetometer/st_mag3d_buffer.c b/drivers/iio/stm/magnetometer/st_mag3d_buffer.c new file mode 100644 index 000000000000..ccfdcf66bafa --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag3d_buffer.c @@ -0,0 +1,169 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_mag3d buffer library driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_mag3d.h" + +#define ST_MAG3D_STATUS_ADDR 0x27 +#define ST_MAG3D_DA_MASK 0x07 + +static const struct iio_trigger_ops st_mag3d_trigger_ops = { + NULL +}; + +static inline s64 st_mag3d_ewma(s64 old, s64 new) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_MAG3D_EWMA_DIV - ST_MAG3D_EWMA_WEIGHT) * diff, + ST_MAG3D_EWMA_DIV); + + return old + incr; +} + +static irqreturn_t st_mag3d_trigger_handler_irq(int irq, void *p) +{ + struct st_mag3d_hw *hw = (struct st_mag3d_hw *)p; + s64 irq_ts; + + irq_ts = st_mag3d_get_time_ns(hw->iio_dev); + + if (hw->stodis == 0) + hw->delta_ts = st_mag3d_ewma(hw->delta_ts, + irq_ts - hw->timestamp); + + hw->timestamp = irq_ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_mag3d_trigger_handler_thread(int irq, void *p) +{ + struct st_mag3d_hw *hw = (struct st_mag3d_hw *)p; + struct iio_dev *iio_dev = hw->iio_dev; + struct iio_chan_spec const *ch = iio_dev->channels; + u8 status; + int err; + + err = hw->tf->read(hw->dev, ST_MAG3D_STATUS_ADDR, sizeof(status), + &status); + if (err < 0) + goto out; + + if (!(status & ST_MAG3D_DA_MASK)) + return IRQ_NONE; + + err = hw->tf->read(hw->dev, ch->address, ST_MAG3D_SAMPLE_SIZE, + hw->buffer); + if (err < 0) + goto out; + + iio_trigger_poll_chained(hw->trig); + +out: + return IRQ_HANDLED; +} + +int st_mag3d_allocate_trigger(struct iio_dev *iio_dev) +{ + struct st_mag3d_hw *hw = iio_priv(iio_dev); + int err; + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_mag3d_trigger_handler_irq, + st_mag3d_trigger_handler_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + "st_mag3d", hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return err; + } + + hw->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger", + iio_dev->name); + if (!hw->trig) + return -ENOMEM; + + iio_trigger_set_drvdata(hw->trig, iio_dev); + hw->trig->ops = &st_mag3d_trigger_ops, + hw->trig->dev.parent = hw->dev; + iio_dev->trig = iio_trigger_get(hw->trig); + + return iio_trigger_register(hw->trig); +} + +void st_mag3d_deallocate_trigger(struct iio_dev *iio_dev) +{ + struct st_mag3d_hw *hw = iio_priv(iio_dev); + + iio_trigger_unregister(hw->trig); +} + +static int st_mag3d_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_mag3d_enable_sensor(iio_priv(iio_dev), true); +} + +static int st_mag3d_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_mag3d_enable_sensor(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_mag3d_buffer_ops = { + .preenable = st_mag3d_buffer_preenable, +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0) + .postenable = iio_triggered_buffer_postenable, + .predisable = iio_triggered_buffer_predisable, +#endif /* LINUX_VERSION_CODE */ + .postdisable = st_mag3d_buffer_postdisable, +}; + +static irqreturn_t st_mag3d_buffer_handler_thread(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct st_mag3d_hw *hw = iio_priv(pf->indio_dev); + + if (hw->stodis == 0) + iio_push_to_buffers_with_timestamp(pf->indio_dev, hw->buffer, + hw->mag_ts); + else + hw->stodis--; + + hw->mag_ts += hw->delta_ts; + iio_trigger_notify_done(hw->trig); + + return IRQ_HANDLED; +} + +int st_mag3d_allocate_buffer(struct iio_dev *iio_dev) +{ + return iio_triggered_buffer_setup(iio_dev, NULL, + st_mag3d_buffer_handler_thread, + &st_mag3d_buffer_ops); +} + +void st_mag3d_deallocate_buffer(struct iio_dev *iio_dev) +{ + iio_triggered_buffer_cleanup(iio_dev); +} + +MODULE_DESCRIPTION("STMicroelectronics mag3d buffer driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/st_mag3d_core.c b/drivers/iio/stm/magnetometer/st_mag3d_core.c new file mode 100644 index 000000000000..3c589b5d3d2e --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag3d_core.c @@ -0,0 +1,465 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics mag3d driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include + +#include "st_mag3d.h" + +#define ST_MAG3D_WHOAMI_ADDR 0x0f +#define ST_MAG3D_DEF_WHOAMI 0x3d + +#define ST_MAG3D_CTRL1_ADDR 0x20 +#define ST_MAG3D_ODR_MASK 0x1c +#define ST_MAG3D_OM_XY_MASK 0x60 +#define ST_MAG3D_CTRL2_ADDR 0x21 +#define ST_MAG3D_FS_MASK 0x60 +#define ST_MAG3D_CTRL3_ADDR 0x22 +#define ST_MAG3D_PWR_MASK 0x03 +#define ST_MAG3D_PWR_ON 0x00 +#define ST_MAG3D_PWR_OFF 0x03 +#define ST_MAG3D_CTRL4_ADDR 0x23 +#define ST_MAG3D_OM_Z_MASK 0x0c +#define ST_MAG3D_CTRL5_ADDR 0x24 +#define ST_MAG3D_BDU_MASK 0x40 + +#define ST_MAG3D_OUT_X_L_ADDR 0x28 +#define ST_MAG3D_OUT_Y_L_ADDR 0x2a +#define ST_MAG3D_OUT_Z_L_ADDR 0x2c + +struct st_mag3d_odr { + unsigned int hz; + u8 val; +}; + +const struct st_mag3d_odr st_mag3d_odr_table[] = { + { 1, 0x01 }, + { 3, 0x02 }, + { 5, 0x03 }, + { 10, 0x04 }, + { 20, 0x05 }, + { 40, 0x06 }, + { 80, 0x07 }, +}; + +const u8 st_mag3d_stodis_table[] = { + 0, /* 1Hz */ + 1, /* 3Hz */ + 2, /* 5Hz */ + 5, /* 10Hz */ + 6, /* 20Hz */ + 6, /* 40Hz */ + 7, /* 80Hz */ +}; + +struct st_mag3d_fs { + unsigned int gain; + u8 val; +}; + +const struct st_mag3d_fs st_mag3d_fs_table[] = { + { 146, 0x0 }, /* 4000 */ + { 292, 0x1 }, /* 8000 */ + { 438, 0x2 }, /* 12000 */ + { 584, 0x3 }, /* 16000 */ +}; + +#define ST_MAG3D_CHANNEL(addr, mod, scan_idx) \ +{ \ + .type = IIO_MAGN, \ + .address = addr, \ + .modified = 1, \ + .channel2 = mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_mag3d_channels[] = { + ST_MAG3D_CHANNEL(ST_MAG3D_OUT_X_L_ADDR, IIO_MOD_X, 0), + ST_MAG3D_CHANNEL(ST_MAG3D_OUT_Y_L_ADDR, IIO_MOD_Y, 1), + ST_MAG3D_CHANNEL(ST_MAG3D_OUT_Z_L_ADDR, IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static inline int st_mag3d_claim_direct_mode(struct iio_dev *iio_dev) +{ + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + + return 0; +} + +static inline void st_mag3d_release_direct_mode(struct iio_dev *iio_dev) +{ + mutex_unlock(&iio_dev->mlock); +} + +static int st_mag3d_write_with_mask(struct st_mag3d_hw *hw, u8 addr, + u8 mask, u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + goto out; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); + if (err < 0) + dev_err(hw->dev, "failed to write %02x register\n", addr); + +out: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_mag3d_set_odr(struct st_mag3d_hw *hw, unsigned int odr) +{ + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_mag3d_odr_table); i++) { + if (odr == st_mag3d_odr_table[i].hz) + break; + } + + if (i == ARRAY_SIZE(st_mag3d_odr_table)) + return -EINVAL; + + err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL1_ADDR, + ST_MAG3D_ODR_MASK, + st_mag3d_odr_table[i].val); + if (err < 0) + return err; + + hw->odr = odr; + hw->stodis = st_mag3d_stodis_table[i]; + hw->delta_ts = div_s64(1000000000LL, odr); + + return 0; +} + +static int st_mag3d_set_fullscale(struct st_mag3d_hw *hw, unsigned int gain) +{ + int i, err; + + for (i = 0; i < ARRAY_SIZE(st_mag3d_fs_table); i++) { + if (gain == st_mag3d_fs_table[i].gain) + break; + } + + if (i == ARRAY_SIZE(st_mag3d_fs_table)) + return -EINVAL; + + err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL2_ADDR, + ST_MAG3D_FS_MASK, + st_mag3d_fs_table[i].val); + if (err < 0) + return err; + + hw->gain = gain; + + return 0; +} + +int st_mag3d_enable_sensor(struct st_mag3d_hw *hw, bool enable) +{ + u8 val = enable ? ST_MAG3D_PWR_ON : ST_MAG3D_PWR_OFF; + int err; + + err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL3_ADDR, + ST_MAG3D_PWR_MASK, val); + if (enable) { + hw->timestamp = st_mag3d_get_time_ns(hw->iio_dev); + hw->mag_ts = hw->timestamp; + } + + return err < 0 ? err : 0; +} + +static int st_mag3d_check_whoami(struct st_mag3d_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_MAG3D_WHOAMI_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_MAG3D_DEF_WHOAMI) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + return 0; +} + +static int st_mag3d_read_oneshot(struct st_mag3d_hw *hw, + u8 addr, int *val) +{ + int err, delay; + __le16 data; + + err = st_mag3d_enable_sensor(hw, true); + if (err < 0) + return err; + + delay = 1000000 / hw->odr; + usleep_range(delay, 2 * delay); + + err = hw->tf->read(hw->dev, addr, sizeof(data), (u8 *)&data); + if (err < 0) + return err; + + err = st_mag3d_enable_sensor(hw, false); + if (err < 0) + return err; + + *val = (s16)data; + + return IIO_VAL_INT; +} + +static int st_mag3d_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_mag3d_hw *hw = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = st_mag3d_claim_direct_mode(iio_dev); + if (ret) + break; + + ret = st_mag3d_read_oneshot(hw, ch->address, val); + st_mag3d_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = hw->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_mag3d_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_mag3d_hw *hw = iio_priv(iio_dev); + int ret; + + ret = st_mag3d_claim_direct_mode(iio_dev); + if (ret) + return ret; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + ret = st_mag3d_set_fullscale(hw, val2); + break; + default: + ret = -EINVAL; + break; + } + + st_mag3d_release_direct_mode(iio_dev); + + return ret; +} + +static ssize_t st_mag3d_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_mag3d_hw *hw = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", hw->odr); +} + +static ssize_t st_mag3d_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, + const char *buf, + size_t count) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag3d_hw *hw = iio_priv(iio_dev); + unsigned int odr; + int err; + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + err = st_mag3d_set_odr(hw, odr); + + return err < 0 ? err : count; +} + +static ssize_t +st_mag3d_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ARRAY_SIZE(st_mag3d_odr_table); i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_mag3d_odr_table[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ(0644, + st_mag3d_get_sampling_frequency, + st_mag3d_set_sampling_frequency); +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_mag3d_get_sampling_frequency_avail); + +static struct attribute *st_mag3d_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_mag3d_attribute_group = { + .attrs = st_mag3d_attributes, +}; + +static const struct iio_info st_mag3d_info = { + .read_raw = st_mag3d_read_raw, + .write_raw = st_mag3d_write_raw, + .attrs = &st_mag3d_attribute_group, +}; + +static const unsigned long st_mag3d_available_scan_masks[] = {0x7, 0x0}; + +int st_mag3d_probe(struct device *dev, int irq, const char *name, + const struct st_mag3d_transfer_function *tf_ops) +{ + struct st_mag3d_hw *hw; + struct iio_dev *iio_dev; + int err; + + iio_dev = devm_iio_device_alloc(dev, sizeof(*hw)); + if (!iio_dev) + return -ENOMEM; + + dev_set_drvdata(dev, iio_dev); + + iio_dev->dev.parent = dev; + iio_dev->name = name; + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->available_scan_masks = st_mag3d_available_scan_masks; + iio_dev->channels = st_mag3d_channels; + iio_dev->num_channels = ARRAY_SIZE(st_mag3d_channels); + iio_dev->info = &st_mag3d_info; + + hw = iio_priv(iio_dev); + mutex_init(&hw->lock); + hw->dev = dev; + hw->tf = tf_ops; + hw->irq = irq; + hw->iio_dev = iio_dev; + + err = st_mag3d_check_whoami(hw); + if (err < 0) + return err; + + err = st_mag3d_set_odr(hw, st_mag3d_odr_table[0].hz); + if (err < 0) + return err; + + err = st_mag3d_set_fullscale(hw, st_mag3d_fs_table[0].gain); + if (err < 0) + return err; + + /* enable BDU */ + err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL5_ADDR, + ST_MAG3D_BDU_MASK, 1); + if (err < 0) + return err; + + /* enable OM mode */ + err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL1_ADDR, + ST_MAG3D_OM_XY_MASK, 3); + if (err < 0) + return err; + + err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL4_ADDR, + ST_MAG3D_OM_Z_MASK, 3); + if (err < 0) + return err; + + if (irq > 0) { + err = st_mag3d_allocate_buffer(iio_dev); + if (err < 0) + return err; + + err = st_mag3d_allocate_trigger(iio_dev); + if (err < 0) { + st_mag3d_deallocate_buffer(iio_dev); + return err; + } + } + + err = devm_iio_device_register(dev, iio_dev); + if (err < 0 && irq > 0) { + st_mag3d_deallocate_trigger(iio_dev); + st_mag3d_deallocate_buffer(iio_dev); + return err; + } + + return 0; +} +EXPORT_SYMBOL(st_mag3d_probe); + +void st_mag3d_remove(struct iio_dev *iio_dev) +{ + struct st_mag3d_hw *hw = iio_priv(iio_dev); + + if (hw->irq > 0) { + st_mag3d_deallocate_trigger(iio_dev); + st_mag3d_deallocate_buffer(iio_dev); + } +} +EXPORT_SYMBOL(st_mag3d_remove); + +MODULE_DESCRIPTION("STMicroelectronics mag3d driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/st_mag3d_i2c.c b/drivers/iio/stm/magnetometer/st_mag3d_i2c.c new file mode 100644 index 000000000000..67b463354850 --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag3d_i2c.c @@ -0,0 +1,90 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics mag3d i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include + +#include "st_mag3d.h" + +#define I2C_AUTO_INCREMENT BIT(7) + +static int st_mag3d_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + if (len > 1) + addr |= I2C_AUTO_INCREMENT; + + return i2c_smbus_read_i2c_block_data_or_emulated(to_i2c_client(dev), + addr, len, data); +} + +static int st_mag3d_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + return i2c_smbus_write_i2c_block_data(to_i2c_client(dev), addr, + len, data); +} + +static const struct st_mag3d_transfer_function st_mag3d_tf_i2c = { + .write = st_mag3d_i2c_write, + .read = st_mag3d_i2c_read, +}; + +static int st_mag3d_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_mag3d_probe(&client->dev, client->irq, client->name, + &st_mag3d_tf_i2c); +} + +static int st_mag3d_i2c_remove(struct i2c_client *client) +{ + struct iio_dev *iio_dev = i2c_get_clientdata(client); + + st_mag3d_remove(iio_dev); + + return 0; +} + +static const struct i2c_device_id st_mag3d_ids[] = { + { LIS3MDL_DEV_NAME }, + { LSM9DS1_DEV_NAME }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_mag3d_ids); + +#ifdef CONFIG_OF +static const struct of_device_id st_mag3d_id_table[] = { + { + .compatible = "st,lis3mdl_magn", + }, + { + .compatible = "st,lsm9ds1_magn", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_mag3d_id_table); +#endif /* CONFIG_OF */ + +static struct i2c_driver st_mag3d_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_mag3d_i2c", +#ifdef CONFIG_OF + .of_match_table = st_mag3d_id_table, +#endif /* CONFIG_OF */ + }, + .probe = st_mag3d_i2c_probe, + .remove = st_mag3d_i2c_remove, + .id_table = st_mag3d_ids, +}; +module_i2c_driver(st_mag3d_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics mag3d i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/st_mag3d_spi.c b/drivers/iio/stm/magnetometer/st_mag3d_spi.c new file mode 100644 index 000000000000..e97c1734a687 --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag3d_spi.c @@ -0,0 +1,124 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics mag3d spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_mag3d.h" + +#define ST_SENSORS_SPI_READ BIT(7) + +static int st_mag3d_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag3d_hw *hw = iio_priv(iio_dev); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + hw->tb.tx_buf[0] = addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(hw->dev), xfers, + ARRAY_SIZE(xfers)); + if (err) + return err; + + memcpy(data, hw->tb.rx_buf, len*sizeof(u8)); + + return len; +} + +static int st_mag3d_spi_write(struct device *dev, u8 addr, int len, u8 *data) +{ + + struct st_mag3d_hw *hw; + struct iio_dev *iio_dev; + + if (len >= ST_MAG3D_TX_MAX_LENGTH) + return -ENOMEM; + + iio_dev = dev_get_drvdata(dev); + hw = iio_priv(iio_dev); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(to_spi_device(hw->dev), hw->tb.tx_buf, len + 1); +} + +static const struct st_mag3d_transfer_function st_mag3d_tf_spi = { + .write = st_mag3d_spi_write, + .read = st_mag3d_spi_read, +}; + +static int st_mag3d_spi_probe(struct spi_device *spi) +{ + return st_mag3d_probe(&spi->dev, spi->irq, spi->modalias, + &st_mag3d_tf_spi); +} + +static int st_mag3d_spi_remove(struct spi_device *spi) +{ + struct iio_dev *iio_dev = spi_get_drvdata(spi); + + st_mag3d_remove(iio_dev); + + return 0; +} + +static const struct spi_device_id st_mag3d_ids[] = { + { LIS3MDL_DEV_NAME }, + { LSM9DS1_DEV_NAME }, + {} +}; +MODULE_DEVICE_TABLE(spi, st_mag3d_ids); + +#ifdef CONFIG_OF +static const struct of_device_id st_mag3d_id_table[] = { + { + .compatible = "st,lis3mdl_magn", + }, + { + .compatible = "st,lsm9ds1_magn", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_mag3d_id_table); +#endif /* CONFIG_OF */ + +static struct spi_driver st_mag3d_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_mag3d_spi", +#ifdef CONFIG_OF + .of_match_table = st_mag3d_id_table, +#endif /* CONFIG_OF */ + }, + .probe = st_mag3d_spi_probe, + .remove = st_mag3d_spi_remove, + .id_table = st_mag3d_ids, +}; +module_spi_driver(st_mag3d_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics mag3d spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/st_mag40_buffer.c b/drivers/iio/stm/magnetometer/st_mag40_buffer.c new file mode 100644 index 000000000000..236143a4803d --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag40_buffer.c @@ -0,0 +1,180 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_mag40 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_mag40_core.h" + +#define ST_MAG40_EWMA_DIV 128 +static inline s64 st_mag40_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_MAG40_EWMA_DIV - weight) * diff, + ST_MAG40_EWMA_DIV); + + return old + incr; +} + +static irqreturn_t st_mag40_trigger_irq_handler(int irq, void *private) +{ + struct st_mag40_data *cdata = private; + struct iio_dev *iio_dev = dev_get_drvdata(cdata->dev); + s64 ts; + u8 weight = (cdata->odr >= 50) ? 96 : 0; + + ts = st_mag40_get_timestamp(iio_dev); + cdata->delta_ts = st_mag40_ewma(cdata->delta_ts, ts - cdata->ts_irq, weight); + cdata->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_mag40_trigger_thread_handler(int irq, void *private) +{ + struct st_mag40_data *cdata = private; + u8 status; + int err; + + err = cdata->tf->read(cdata, ST_MAG40_STATUS_ADDR, + sizeof(status), &status); + if (err < 0) + return IRQ_HANDLED; + + if (!(status & ST_MAG40_AVL_DATA_MASK)) + return IRQ_NONE; + + iio_trigger_poll_chained(cdata->iio_trig); + + return IRQ_HANDLED; +} + +static irqreturn_t st_mag40_buffer_thread_handler(int irq, void *p) +{ + u8 buffer[ALIGN(ST_MAG40_OUT_LEN, sizeof(s64)) + sizeof(s64)]; + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct st_mag40_data *cdata = iio_priv(iio_dev); + int err; + + err = cdata->tf->read(cdata, ST_MAG40_OUTX_L_ADDR, + ST_MAG40_OUT_LEN, buffer); + if (err < 0) + goto out; + + /* discard samples generated during the turn-on time */ + if (cdata->samples_to_discard > 0) { + cdata->samples_to_discard--; + goto out; + } + + iio_push_to_buffers_with_timestamp(iio_dev, buffer, cdata->ts); + cdata->ts += cdata->delta_ts; + +out: + iio_trigger_notify_done(cdata->iio_trig); + + return IRQ_HANDLED; +} + +static int st_mag40_buffer_preenable(struct iio_dev *indio_dev) +{ + struct st_mag40_data *cdata = iio_priv(indio_dev); + + return st_mag40_set_enable(cdata, true); +} + +static int st_mag40_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct st_mag40_data *cdata = iio_priv(indio_dev); + int err; + + err = st_mag40_set_enable(cdata, false); + + return err < 0 ? err : 0; +} + +static const struct iio_buffer_setup_ops st_mag40_buffer_setup_ops = { + .preenable = st_mag40_buffer_preenable, + .postdisable = st_mag40_buffer_postdisable, +}; + +int st_mag40_trig_set_state(struct iio_trigger *trig, bool state) +{ + struct st_mag40_data *cdata = iio_priv(iio_trigger_get_drvdata(trig)); + int err; + + err = st_mag40_write_register(cdata, ST_MAG40_INT_DRDY_ADDR, + ST_MAG40_INT_DRDY_MASK, state); + + return err < 0 ? err : 0; +} + +int st_mag40_allocate_ring(struct iio_dev *iio_dev) +{ + return iio_triggered_buffer_setup(iio_dev, NULL, + st_mag40_buffer_thread_handler, + &st_mag40_buffer_setup_ops); +} + +void st_mag40_deallocate_ring(struct iio_dev *iio_dev) +{ + iio_triggered_buffer_cleanup(iio_dev); +} + +static const struct iio_trigger_ops st_mag40_trigger_ops = { + .set_trigger_state = st_mag40_trig_set_state, +}; + +int st_mag40_allocate_trigger(struct iio_dev *iio_dev) +{ + struct st_mag40_data *cdata = iio_priv(iio_dev); + int err; + + err = devm_request_threaded_irq(cdata->dev, cdata->irq, + st_mag40_trigger_irq_handler, st_mag40_trigger_thread_handler, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + cdata->name, cdata); + if (err) + return err; + + cdata->iio_trig = devm_iio_trigger_alloc(cdata->dev, "%s-trigger", + iio_dev->name); + if (!cdata->iio_trig) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + return -ENOMEM; + } + iio_trigger_set_drvdata(cdata->iio_trig, iio_dev); + cdata->iio_trig->ops = &st_mag40_trigger_ops; + cdata->iio_trig->dev.parent = cdata->dev; + + err = iio_trigger_register(cdata->iio_trig); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + return err; + } + iio_dev->trig = cdata->iio_trig; + + return 0; +} + +void st_mag40_deallocate_trigger(struct st_mag40_data *cdata) +{ + iio_trigger_unregister(cdata->iio_trig); +} diff --git a/drivers/iio/stm/magnetometer/st_mag40_core.c b/drivers/iio/stm/magnetometer/st_mag40_core.c new file mode 100644 index 000000000000..e5f2341e5e00 --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag40_core.c @@ -0,0 +1,389 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_mag40 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_mag40_core.h" + +struct st_mag40_odr_reg { + u32 hz; + u8 value; +}; + +#define ST_MAG40_ODR_TABLE_SIZE 4 +static const struct st_mag40_odr_table_t { + u8 addr; + u8 mask; + struct st_mag40_odr_reg odr_avl[ST_MAG40_ODR_TABLE_SIZE]; +} st_mag40_odr_table = { + .addr = ST_MAG40_ODR_ADDR, + .mask = ST_MAG40_ODR_MASK, + .odr_avl[0] = { .hz = 10, .value = ST_MAG40_CFG_REG_A_ODR_10Hz, }, + .odr_avl[1] = { .hz = 20, .value = ST_MAG40_CFG_REG_A_ODR_20Hz, }, + .odr_avl[2] = { .hz = 50, .value = ST_MAG40_CFG_REG_A_ODR_50Hz, }, + .odr_avl[3] = { .hz = 100, .value = ST_MAG40_CFG_REG_A_ODR_100Hz, }, +}; + +#define ST_MAG40_ADD_CHANNEL(device_type, modif, index, mod, \ + endian, sbits, rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +static const struct iio_chan_spec st_mag40_channels[] = { + ST_MAG40_ADD_CHANNEL(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, 16, 16, + ST_MAG40_OUTX_L_ADDR, 's'), + ST_MAG40_ADD_CHANNEL(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, 16, 16, + ST_MAG40_OUTY_L_ADDR, 's'), + ST_MAG40_ADD_CHANNEL(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, 16, 16, + ST_MAG40_OUTZ_L_ADDR, 's'), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +int st_mag40_write_register(struct st_mag40_data *cdata, u8 reg_addr, + u8 mask, u8 data) +{ + int err; + u8 val; + + mutex_lock(&cdata->lock); + + err = cdata->tf->read(cdata, reg_addr, sizeof(val), &val); + if (err < 0) + goto unlock; + + val = ((val & ~mask) | ((data << __ffs(mask)) & mask)); + + err = cdata->tf->write(cdata, reg_addr, sizeof(val), &val); + +unlock: + mutex_unlock(&cdata->lock); + + return err < 0 ? err : 0; +} + +static int st_mag40_write_odr(struct st_mag40_data *cdata, uint32_t odr) +{ + int err, i; + + for (i = 0; i < ST_MAG40_ODR_TABLE_SIZE; i++) + if (st_mag40_odr_table.odr_avl[i].hz >= odr) + break; + + if (i == ST_MAG40_ODR_TABLE_SIZE) + return -EINVAL; + + err = st_mag40_write_register(cdata, st_mag40_odr_table.addr, + st_mag40_odr_table.mask, + st_mag40_odr_table.odr_avl[i].value); + if (err < 0) + return err; + + cdata->odr = odr; + cdata->samples_to_discard = ST_MAG40_TURNON_TIME_SAMPLES_NUM; + + return 0; +} + +int st_mag40_set_enable(struct st_mag40_data *cdata, bool state) +{ + struct iio_dev *iio_dev = dev_get_drvdata(cdata->dev); + u8 mode; + + mode = state ? ST_MAG40_CFG_REG_A_MD_CONT : ST_MAG40_CFG_REG_A_MD_IDLE; + + if (state) { + cdata->ts = cdata->ts_irq = st_mag40_get_timestamp(iio_dev); + cdata->delta_ts = div_s64(1000000000LL, cdata->odr); + } + + return st_mag40_write_register(cdata, ST_MAG40_EN_ADDR, + ST_MAG40_EN_MASK, mode); +} + +int st_mag40_init_sensors(struct st_mag40_data *cdata) +{ + int err; + + /* + * Enable block data update feature. + */ + err = st_mag40_write_register(cdata, ST_MAG40_CFG_REG_C_ADDR, + ST_MAG40_CFG_REG_C_BDU_MASK, 1); + if (err < 0) + return err; + + /* + * Enable the temperature compensation feature + */ + err = st_mag40_write_register(cdata, ST_MAG40_CFG_REG_A_ADDR, + ST_MAG40_TEMP_COMP_EN, 1); + if (err < 0) + return err; + + err = st_mag40_write_register(cdata, ST_MAG40_CFG_REG_B_ADDR, + ST_MAG40_CFG_REG_B_OFF_CANC_MASK, 1); + + return err < 0 ? err : 0; +} + +static ssize_t st_mag40_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_mag40_data *cdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", cdata->odr); +} + +static ssize_t st_mag40_set_sampling_frequency(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag40_data *cdata = iio_priv(iio_dev); + unsigned int odr; + int err; + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + err = st_mag40_write_odr(cdata, odr); + + return err < 0 ? err : count; +} + +static ssize_t +st_mag40_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < ST_MAG40_ODR_TABLE_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_mag40_odr_table.odr_avl[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static int st_mag40_read_oneshot(struct st_mag40_data *cdata, + u8 addr, int *val) +{ + u8 data[2]; + int err; + + err = st_mag40_set_enable(cdata, true); + if (err < 0) + return err; + + msleep(40); + + err = cdata->tf->read(cdata, addr, sizeof(data), data); + if (err < 0) + return err; + + *val = (s16)get_unaligned_le16(data); + + err = st_mag40_set_enable(cdata, false); + + return err < 0 ? err : IIO_VAL_INT; +} + +static int st_mag40_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_mag40_data *cdata = iio_priv(iio_dev); + int ret; + + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = st_mag40_read_oneshot(cdata, ch->address, val); + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = 1500; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return ret; +} + +static ssize_t st_mag40_get_module_id(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag40_data *cdata = iio_priv(iio_dev); + + return scnprintf(buf, PAGE_SIZE, "%u\n", cdata->module_id); +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_mag40_get_sampling_frequency, + st_mag40_set_sampling_frequency); +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_mag40_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(module_id, 0444, st_mag40_get_module_id, NULL, 0); + +static struct attribute *st_mag40_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_module_id.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_mag40_attribute_group = { + .attrs = st_mag40_attributes, +}; + +static const struct iio_info st_mag40_info = { + .attrs = &st_mag40_attribute_group, + .read_raw = &st_mag40_read_raw, +}; + +static void st_mag40_get_properties(struct st_mag40_data *cdata) +{ + if (device_property_read_u32(cdata->dev, "st,module_id", + &cdata->module_id)) { + cdata->module_id = 1; + } +} + +int st_mag40_common_probe(struct iio_dev *iio_dev) +{ + struct st_mag40_data *cdata = iio_priv(iio_dev); + int32_t err; + u8 wai; + + mutex_init(&cdata->lock); + + err = cdata->tf->read(cdata, ST_MAG40_WHO_AM_I_ADDR, + sizeof(wai), &wai); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + + if (wai != ST_MAG40_WHO_AM_I_DEF) { + dev_err(cdata->dev, "Who-Am-I value not valid. (%02x)\n", wai); + return -ENODEV; + } + + cdata->odr = st_mag40_odr_table.odr_avl[0].hz; + + iio_dev->channels = st_mag40_channels; + iio_dev->num_channels = ARRAY_SIZE(st_mag40_channels); + iio_dev->info = &st_mag40_info; + iio_dev->modes = INDIO_DIRECT_MODE; + + st_mag40_get_properties(cdata); + + err = st_mag40_init_sensors(cdata); + if (err < 0) + return err; + + if (cdata->irq > 0) { + err = st_mag40_allocate_ring(iio_dev); + if (err < 0) + return err; + + err = st_mag40_allocate_trigger(iio_dev); + if (err < 0) + goto deallocate_ring; + } + + err = devm_iio_device_register(cdata->dev, iio_dev); + if (err) + goto iio_trigger_deallocate; + + return 0; + +iio_trigger_deallocate: + st_mag40_deallocate_trigger(cdata); + +deallocate_ring: + st_mag40_deallocate_ring(iio_dev); + + return err; +} +EXPORT_SYMBOL(st_mag40_common_probe); + +void st_mag40_common_remove(struct iio_dev *iio_dev) +{ + struct st_mag40_data *cdata = iio_priv(iio_dev); + + if (cdata->irq > 0) { + st_mag40_deallocate_trigger(cdata); + st_mag40_deallocate_ring(iio_dev); + } +} +EXPORT_SYMBOL(st_mag40_common_remove); + +#ifdef CONFIG_PM +int st_mag40_common_suspend(struct st_mag40_data *cdata) +{ + return 0; +} +EXPORT_SYMBOL(st_mag40_common_suspend); + +int st_mag40_common_resume(struct st_mag40_data *cdata) +{ + return 0; +} +EXPORT_SYMBOL(st_mag40_common_resume); +#endif /* CONFIG_PM */ + +MODULE_DESCRIPTION("STMicroelectronics st_mag40 driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/st_mag40_core.h b/drivers/iio/stm/magnetometer/st_mag40_core.h new file mode 100644 index 000000000000..358447c216bb --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag40_core.h @@ -0,0 +1,135 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_mag40 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#ifndef __ST_MAG40_H +#define __ST_MAG40_H + +#include +#include +#include + +#define ST_MAG40_DEV_NAME "st_mag40" +#define LIS2MDL_DEV_NAME "lis2mdl_magn" +#define LSM303AH_DEV_NAME "lsm303ah_magn" +#define LSM303AGR_DEV_NAME "lsm303agr_magn" +#define ISM303DAC_DEV_NAME "ism303dac_magn" +#define IIS2MDC_DEV_NAME "iis2mdc_magn" + +/* Power Modes */ +enum { + ST_MAG40_LWC_MODE = 0, + ST_MAG40_NORMAL_MODE, + ST_MAG40_MODE_COUNT, +}; + +#define ST_MAG40_WHO_AM_I_ADDR 0x4f +#define ST_MAG40_WHO_AM_I_DEF 0x40 + +/* Magnetometer control registers */ +#define ST_MAG40_CFG_REG_A_ADDR 0x60 +#define ST_MAG40_TEMP_COMP_EN 0x80 +#define ST_MAG40_CFG_REG_A_ODR_MASK 0x0c +#define ST_MAG40_CFG_REG_A_ODR_10Hz 0x00 +#define ST_MAG40_CFG_REG_A_ODR_20Hz 0x01 +#define ST_MAG40_CFG_REG_A_ODR_50Hz 0x02 +#define ST_MAG40_CFG_REG_A_ODR_100Hz 0x03 +#define ST_MAG40_CFG_REG_A_ODR_COUNT 4 +#define ST_MAG40_CFG_REG_A_MD_MASK 0x03 +#define ST_MAG40_CFG_REG_A_MD_CONT 0x00 +#define ST_MAG40_CFG_REG_A_MD_IDLE 0x03 + +#define ST_MAG40_ODR_ADDR ST_MAG40_CFG_REG_A_ADDR +#define ST_MAG40_ODR_MASK ST_MAG40_CFG_REG_A_ODR_MASK + +#define ST_MAG40_EN_ADDR ST_MAG40_CFG_REG_A_ADDR +#define ST_MAG40_EN_MASK ST_MAG40_CFG_REG_A_MD_MASK + +#define ST_MAG40_CFG_REG_B_ADDR 0x61 +#define ST_MAG40_CFG_REG_B_OFF_CANC_MASK 0x02 + +#define ST_MAG40_CFG_REG_C_ADDR 0x62 +#define ST_MAG40_CFG_REG_C_BDU_MASK 0x10 +#define ST_MAG40_CFG_REG_C_INT_MASK 0x01 + +#define ST_MAG40_INT_DRDY_ADDR ST_MAG40_CFG_REG_C_ADDR +#define ST_MAG40_INT_DRDY_MASK ST_MAG40_CFG_REG_C_INT_MASK + +#define ST_MAG40_STATUS_ADDR 0x67 +#define ST_MAG40_AVL_DATA_MASK 0x7 + +/* Magnetometer output registers */ +#define ST_MAG40_OUTX_L_ADDR 0x68 +#define ST_MAG40_OUTY_L_ADDR 0x6A +#define ST_MAG40_OUTZ_L_ADDR 0x6C + +#define ST_MAG40_BDU_ADDR ST_MAG40_CTRL1_ADDR +#define ST_MAG40_BDU_MASK 0x02 + +#define ST_MAG40_TURNON_TIME_SAMPLES_NUM 2 + +/* 3 axis of 16 bit each */ +#define ST_MAG40_OUT_LEN 6 + +#define ST_MAG40_TX_MAX_LENGTH 16 +#define ST_MAG40_RX_MAX_LENGTH 16 + +struct st_mag40_transfer_buffer { + u8 rx_buf[ST_MAG40_RX_MAX_LENGTH]; + u8 tx_buf[ST_MAG40_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_mag40_data; + +struct st_mag40_transfer_function { + int (*write)(struct st_mag40_data *cdata, u8 reg_addr, int len, u8 *data); + int (*read)(struct st_mag40_data *cdata, u8 reg_addr, int len, u8 *data); +}; + +struct st_mag40_data { + const char *name; + struct mutex lock; + u8 drdy_int_pin; + int irq; + s64 ts; + s64 ts_irq; + s64 delta_ts; + + u32 module_id; + + u16 odr; + u8 samples_to_discard; + + struct device *dev; + struct iio_trigger *iio_trig; + const struct st_mag40_transfer_function *tf; + struct st_mag40_transfer_buffer tb; +}; + +static inline s64 st_mag40_get_timestamp(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +int st_mag40_common_probe(struct iio_dev *iio_dev); +void st_mag40_common_remove(struct iio_dev *iio_dev); + +#ifdef CONFIG_PM +int st_mag40_common_suspend(struct st_mag40_data *cdata); +int st_mag40_common_resume(struct st_mag40_data *cdata); +#endif /* CONFIG_PM */ + +int st_mag40_allocate_ring(struct iio_dev *iio_dev); +int st_mag40_allocate_trigger(struct iio_dev *iio_dev); +int st_mag40_trig_set_state(struct iio_trigger *trig, bool state); +int st_mag40_set_enable(struct st_mag40_data *cdata, bool enable); +void st_mag40_deallocate_ring(struct iio_dev *iio_dev); +void st_mag40_deallocate_trigger(struct st_mag40_data *cdata); +int st_mag40_write_register(struct st_mag40_data *cdata, u8 reg_addr, u8 mask, u8 data); + +#endif /* __ST_MAG40_H */ diff --git a/drivers/iio/stm/magnetometer/st_mag40_i2c.c b/drivers/iio/stm/magnetometer/st_mag40_i2c.c new file mode 100644 index 000000000000..b27f79eb5100 --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag40_i2c.c @@ -0,0 +1,180 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_mag40 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_mag40_core.h" + +#define I2C_AUTO_INCREMENT 0x80 + +static int st_mag40_i2c_read(struct st_mag40_data *cdata, u8 reg_addr, + int len, u8 * data) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg[2]; + + if (len > 1) + reg_addr |= I2C_AUTO_INCREMENT; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_mag40_i2c_write(struct st_mag40_data *cdata, u8 reg_addr, + int len, u8 * data) +{ + struct i2c_client *client = to_i2c_client(cdata->dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_mag40_transfer_function st_mag40_tf_i2c = { + .write = st_mag40_i2c_write, + .read = st_mag40_i2c_read, +}; + +static int st_mag40_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct st_mag40_data *cdata; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(&client->dev, sizeof(*cdata)); + if (!iio_dev) + return -ENOMEM; + + i2c_set_clientdata(client, iio_dev); + iio_dev->dev.parent = &client->dev; + iio_dev->name = client->name; + + cdata = iio_priv(iio_dev); + cdata->dev = &client->dev; + cdata->name = client->name; + cdata->tf = &st_mag40_tf_i2c; + cdata->irq = client->irq; + + return st_mag40_common_probe(iio_dev); +} + +static int st_mag40_i2c_remove(struct i2c_client *client) +{ + struct iio_dev *iio_dev = i2c_get_clientdata(client); + + st_mag40_common_remove(iio_dev); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_mag40_i2c_suspend(struct device *dev) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag40_data *cdata = iio_priv(iio_dev); + + return st_mag40_common_suspend(cdata); +} + +static int __maybe_unused st_mag40_i2c_resume(struct device *dev) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag40_data *cdata = iio_priv(iio_dev); + + return st_mag40_common_resume(cdata); +} + +static const struct dev_pm_ops st_mag40_i2c_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_mag40_i2c_suspend, st_mag40_i2c_resume) +}; +#define ST_MAG40_PM_OPS (&st_mag40_i2c_pm_ops) +#else /* CONFIG_PM */ +#define ST_MAG40_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id st_mag40_ids[] = { + { LSM303AH_DEV_NAME, 0 }, + { LSM303AGR_DEV_NAME, 0 }, + { LIS2MDL_DEV_NAME, 0 }, + { ISM303DAC_DEV_NAME, 0 }, + { IIS2MDC_DEV_NAME, 0 }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_mag40_ids); + +#ifdef CONFIG_OF +static const struct of_device_id st_mag40_id_table[] = { + { + .compatible = "st,lsm303ah_magn", + .data = LSM303AH_DEV_NAME, + }, + { + .compatible = "st,lsm303agr_magn", + .data = LSM303AGR_DEV_NAME, + }, + { + .compatible = "st,lis2mdl_magn", + .data = LSM303AGR_DEV_NAME, + }, + { + .compatible = "st,ism303dac_magn", + .data = ISM303DAC_DEV_NAME, + }, + { + .compatible = "st,iis2mdc_magn", + .data = IIS2MDC_DEV_NAME, + }, + {}, +}; + +MODULE_DEVICE_TABLE(of, st_mag40_id_table); +#endif /* CONFIG_OF */ + +static struct i2c_driver st_mag40_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = ST_MAG40_DEV_NAME, + .pm = ST_MAG40_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = st_mag40_id_table, +#endif /* CONFIG_OF */ + }, + .probe = st_mag40_i2c_probe, + .remove = st_mag40_i2c_remove, + .id_table = st_mag40_ids, +}; +module_i2c_driver(st_mag40_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics st_mag40 i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/magnetometer/st_mag40_spi.c b/drivers/iio/stm/magnetometer/st_mag40_spi.c new file mode 100644 index 000000000000..c75f2494d66b --- /dev/null +++ b/drivers/iio/stm/magnetometer/st_mag40_spi.c @@ -0,0 +1,185 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_mag40 driver + * + * MEMS Software Solutions Team + * + * Copyright 2016 STMicroelectronics Inc. + */ + +#include +#include +#include +#include + +#include "st_mag40_core.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_mag40_spi_read(struct st_mag40_data *cdata, + u8 reg_addr, int len, u8 *data) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + return err; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + + return len; +} + +static int st_mag40_spi_write(struct st_mag40_data *cdata, + u8 reg_addr, int len, u8 *data) +{ + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_MAG40_RX_MAX_LENGTH) + return -ENOMEM; + + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + return spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); +} + +static const struct st_mag40_transfer_function st_mag40_tf_spi = { + .write = st_mag40_spi_write, + .read = st_mag40_spi_read, +}; + +static int st_mag40_spi_probe(struct spi_device *spi) +{ + struct st_mag40_data *cdata; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*cdata)); + if (!iio_dev) + return -ENOMEM; + + spi_set_drvdata(spi, iio_dev); + iio_dev->dev.parent = &spi->dev; + iio_dev->name = spi->modalias; + + cdata = iio_priv(iio_dev); + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + cdata->tf = &st_mag40_tf_spi; + cdata->irq = spi->irq; + + return st_mag40_common_probe(iio_dev); +} + +static int st_mag40_spi_remove(struct spi_device *spi) +{ + struct iio_dev *iio_dev = spi_get_drvdata(spi); + + st_mag40_common_remove(iio_dev); + + return 0; +} + +#ifdef CONFIG_PM +static int __maybe_unused st_mag40_spi_suspend(struct device *dev) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag40_data *cdata = iio_priv(iio_dev); + + return st_mag40_common_suspend(cdata); +} + +static int __maybe_unused st_mag40_spi_resume(struct device *dev) +{ + struct iio_dev *iio_dev = dev_get_drvdata(dev); + struct st_mag40_data *cdata = iio_priv(iio_dev); + + return st_mag40_common_resume(cdata); +} + +static const struct dev_pm_ops st_mag40_spi_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_mag40_spi_suspend, st_mag40_spi_resume) +}; +#define ST_MAG40_PM_OPS (&st_mag40_spi_pm_ops) +#else /* CONFIG_PM */ +#define ST_MAG40_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id st_mag40_ids[] = { + { LSM303AH_DEV_NAME, 0 }, + { LSM303AGR_DEV_NAME, 0 }, + { LIS2MDL_DEV_NAME, 0 }, + { ISM303DAC_DEV_NAME, 0 }, + { IIS2MDC_DEV_NAME, 0 }, + {} +}; + +MODULE_DEVICE_TABLE(spi, st_mag40_ids); + +#ifdef CONFIG_OF +static const struct of_device_id st_mag40_id_table[] = { + { + .compatible = "st,lsm303ah_magn", + .data = LSM303AH_DEV_NAME, + }, + { + .compatible = "st,lsm303agr_magn", + .data = LSM303AGR_DEV_NAME, + }, + { + .compatible = "st,lis2mdl_magn", + .data = LSM303AGR_DEV_NAME, + }, + { + .compatible = "st,ism303dac_magn", + .data = ISM303DAC_DEV_NAME, + }, + { + .compatible = "st,iis2mdc_magn", + .data = IIS2MDC_DEV_NAME, + }, + {}, +}; + +MODULE_DEVICE_TABLE(of, st_mag40_id_table); +#endif /* CONFIG_OF */ + +static struct spi_driver st_mag40_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = ST_MAG40_DEV_NAME, + .pm = ST_MAG40_PM_OPS, +#ifdef CONFIG_OF + .of_match_table = st_mag40_id_table, +#endif /* CONFIG_OF */ + }, + .probe = st_mag40_spi_probe, + .remove = st_mag40_spi_remove, + .id_table = st_mag40_ids, +}; +module_spi_driver(st_mag40_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics st_mag40 spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/Kconfig b/drivers/iio/stm/pressure/Kconfig new file mode 100644 index 000000000000..3504e056abbf --- /dev/null +++ b/drivers/iio/stm/pressure/Kconfig @@ -0,0 +1,88 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Pressure drivers +# +# When adding new entries keep the list in alphabetical order + +menu "Pressure sensors" + +config ST_LPS22HH_IIO + tristate "STMicroelectronics LPS22CH/LPS22HH/LPS27HHW sensor" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_KFIFO_BUF + select ST_LPS22HH_I2C_IIO if (I2C) + select ST_LPS22HH_SPI_IIO if (SPI) + help + This driver supports LPS22HH sensors. This driver can be + built as a module. The module will be called st-lps22hh. + +config ST_LPS22HH_I2C_IIO + tristate + depends on ST_LPS22HH_IIO + +config ST_LPS22HH_SPI_IIO + tristate + depends on ST_LPS22HH_IIO + +config ST_LPS22DF_IIO + tristate "STMicroelectronics LPS22DF/LPS28DFW sensor" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_KFIFO_BUF + select ST_LPS22DF_I2C_IIO if (I2C) + select ST_LPS22DF_SPI_IIO if (SPI) + help + Say yes here to build support for the ST MEMS LPS22DF/LPS28DFW + pressure and temperature sensor. + + This driver can be built as a module. The module will be called + st-lps22df. + +config ST_LPS22DF_I2C_IIO + tristate + depends on ST_LPS22DF_IIO + +config ST_LPS22DF_SPI_IIO + tristate + depends on ST_LPS22DF_IIO + +config ST_LPS22HB_IIO + tristate "STMicroelectronics LPS22HB sensor" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_KFIFO_BUF + select ST_LPS22HB_I2C_IIO if (I2C) + select ST_LPS22HB_SPI_IIO if (SPI) + help + This driver supports LPS22HB pressure sensor. This driver can be + built as a module. The module will be called st-lps22hb. + +config ST_LPS22HB_I2C_IIO + tristate + depends on ST_LPS22HB_IIO + +config ST_LPS22HB_SPI_IIO + tristate + depends on ST_LPS22HB_IIO + +config ST_LPS33HW_IIO + tristate "STMicroelectronics LPS33HW sensor" + depends on (I2C || SPI_MASTER) && SYSFS + select IIO_BUFFER + select IIO_KFIFO_BUF + select ST_LPS33HW_I2C_IIO if (I2C) + select ST_LPS33HW_SPI_IIO if (SPI) + help + This driver supports LPS33HW pressure sensors. This driver can be + built as a module. The module will be called st-lps33hw. + +config ST_LPS33HW_I2C_IIO + tristate + depends on ST_LPS33HW_IIO + +config ST_LPS33HW_SPI_IIO + tristate + depends on ST_LPS33HW_IIO + +endmenu diff --git a/drivers/iio/stm/pressure/Makefile b/drivers/iio/stm/pressure/Makefile new file mode 100644 index 000000000000..e546baf86ed6 --- /dev/null +++ b/drivers/iio/stm/pressure/Makefile @@ -0,0 +1,29 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for industrial I/O pressure drivers +# + +# When adding new entries keep the list in alphabetical order +obj-$(CONFIG_ST_LPS22HH_IIO) += st_lps22hh.o +obj-$(CONFIG_ST_LPS22HH_I2C_IIO) += st_lps22hh_i2c.o +obj-$(CONFIG_ST_LPS22HH_SPI_IIO) += st_lps22hh_spi.o + +st_lps22hh-y += st_lps22hh_core.o st_lps22hh_buffer.o + +obj-$(CONFIG_ST_LPS22DF_IIO) += st_lps22df.o +obj-$(CONFIG_ST_LPS22DF_I2C_IIO) += st_lps22df_i2c.o +obj-$(CONFIG_ST_LPS22DF_SPI_IIO) += st_lps22df_spi.o + +st_lps22df-y += st_lps22df_core.o st_lps22df_buffer.o + +obj-$(CONFIG_ST_LPS22HB_IIO) += st_lps22hb.o +obj-$(CONFIG_ST_LPS22HB_I2C_IIO) += st_lps22hb_i2c.o +obj-$(CONFIG_ST_LPS22HB_SPI_IIO) += st_lps22hb_spi.o + +st_lps22hb-y += st_lps22hb_core.o st_lps22hb_buffer.o + +obj-$(CONFIG_ST_LPS33HW_IIO) += st_lps33hw.o +obj-$(CONFIG_ST_LPS33HW_I2C_IIO) += st_lps33hw_i2c.o +obj-$(CONFIG_ST_LPS33HW_SPI_IIO) += st_lps33hw_spi.o + +st_lps33hw-y += st_lps33hw_core.o st_lps33hw_buffer.o diff --git a/drivers/iio/stm/pressure/st_lps22df.h b/drivers/iio/stm/pressure/st_lps22df.h new file mode 100644 index 000000000000..ef6ced54f910 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22df.h @@ -0,0 +1,138 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lps22df driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#ifndef __ST_LPS22DF_H +#define __ST_LPS22DF_H + +#include +#include +#include +#include + +#define ST_LPS22DF_MAX_FIFO_LENGTH 127 + +#define ST_LPS22DF_INTERRUPT_CFG_ADDR 0x0b +#define ST_LPS22DF_LIR_MASK BIT(2) + +#define ST_LPS22DF_WHO_AM_I_ADDR 0x0f +#define ST_LPS22DF_WHO_AM_I_VAL 0xb4 + +#define ST_LPS22DF_CTRL_REG1_ADDR 0x10 +#define ST_LPS22DF_AVG_MASK GENMASK(2, 0) +#define ST_LPS22DF_ODR_MASK GENMASK(6, 3) + +#define ST_LPS22DF_CTRL_REG2_ADDR 0x11 +#define ST_LPS22DF_SWRESET_MASK BIT(2) +#define ST_LPS22DF_BDU_MASK BIT(3) +#define ST_LPS22DF_EN_LPFP_MASK BIT(4) +#define ST_LPS22DF_BOOT_MASK BIT(7) + +#define ST_LPS22DF_CTRL3_ADDR 0x12 +#define ST_LPS22DF_IF_ADD_INC_MASK BIT(0) +#define ST_LPS22DF_PP_OD_MASK BIT(1) +#define ST_LPS22DF_INT_H_L_MASK BIT(3) + +#define ST_LPS22DF_CTRL4_ADDR 0x13 +#define ST_LPS22DF_INT_F_WTM_MASK BIT(1) + +#define ST_LPS22DF_FIFO_CTRL_ADDR 0x14 +#define ST_LPS22DF_FIFO_MODE_MASK GENMASK(1, 0) + +#define ST_LPS22DF_FIFO_WTM_ADDR 0x15 +#define ST_LPS22DF_FIFO_THS_MASK GENMASK(6, 0) + +#define ST_LPS22DF_FIFO_STATUS1_ADDR 0x25 +#define ST_LPS22DF_FIFO_SRC_DIFF_MASK GENMASK(7, 0) + +#define ST_LPS22DF_FIFO_STATUS2_ADDR 0x26 +#define ST_LPS22DF_FIFO_WTM_IA_MASK BIT(7) + +#define ST_LPS22DF_PRESS_OUT_XL_ADDR 0x28 + +#define ST_LPS22DF_TEMP_OUT_L_ADDR 0x2b + +#define ST_LPS22DF_FIFO_DATA_OUT_PRESS_XL_ADDR 0x78 + +#define ST_LPS22DF_PRESS_FS_AVL_GAIN (1000000000UL / 4096UL) +#define ST_LPS22DF_TEMP_FS_AVL_GAIN 100 + +#define ST_LPS22DF_ODR_LIST_NUM 9 + +enum st_lps22df_sensor_type { + ST_LPS22DF_PRESS = 0, + ST_LPS22DF_TEMP, + ST_LPS22DF_SENSORS_NUMB, +}; + +enum st_lps22df_fifo_mode { + ST_LPS22DF_BYPASS = 0x0, + ST_LPS22DF_STREAM = 0x2, +}; + +#define ST_LPS22DF_PRESS_SAMPLE_LEN 3 +#define ST_LPS22DF_TEMP_SAMPLE_LEN 2 + +#define ST_LPS22DF_TX_MAX_LENGTH 64 +#define ST_LPS22DF_RX_MAX_LENGTH ((ST_LPS22DF_MAX_FIFO_LENGTH + 1) * \ + ST_LPS22DF_PRESS_SAMPLE_LEN) + +struct st_lps22df_transfer_buffer { + u8 rx_buf[ST_LPS22DF_RX_MAX_LENGTH]; + u8 tx_buf[ST_LPS22DF_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_lps22df_transfer_function { + int (*write)(struct device *dev, u8 addr, int len, u8 *data); + int (*read)(struct device *dev, u8 addr, int len, u8 *data); +}; + +struct st_lps22df_hw { + struct device *dev; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + u8 watermark; + + struct iio_dev *iio_devs[ST_LPS22DF_SENSORS_NUMB]; + u8 enable_mask; + u8 odr; + + s64 last_fifo_ts; + s64 delta_ts; + s64 ts_irq; + s64 ts; + + const struct st_lps22df_transfer_function *tf; + struct st_lps22df_transfer_buffer tb; +}; + +struct st_lps22df_sensor { + struct st_lps22df_hw *hw; + enum st_lps22df_sensor_type type; + char name[32]; + + u32 gain; + u8 odr; +}; + +int st_lps22df_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps22df_transfer_function *tf_ops); +int st_lps22df_write_with_mask(struct st_lps22df_hw *hw, u8 addr, u8 mask, + u8 data); +int st_lps22df_allocate_buffers(struct st_lps22df_hw *hw); +int st_lps22df_set_enable(struct st_lps22df_sensor *sensor, bool enable); +ssize_t st_lps22df_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count); +ssize_t st_lps22df_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); + +#endif /* __ST_LPS22DF_H */ diff --git a/drivers/iio/stm/pressure/st_lps22df_buffer.c b/drivers/iio/stm/pressure/st_lps22df_buffer.c new file mode 100644 index 000000000000..c39ac3764df1 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22df_buffer.c @@ -0,0 +1,307 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22df buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lps22df.h" + +#define ST_LPS22DF_EWMA_LEVEL 96 +#define ST_LPS22DF_EWMA_DIV 128 + +static inline s64 st_lps22df_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LPS22DF_EWMA_DIV - weight) * diff, + ST_LPS22DF_EWMA_DIV); + + return old + incr; +} + +static inline s64 st_lps22df_get_time_ns(struct st_lps22df_hw *hw) +{ + return iio_get_time_ns(hw->iio_devs[ST_LPS22DF_PRESS]); +} + +static int st_lps22df_set_fifo_mode(struct st_lps22df_hw *hw, + enum st_lps22df_fifo_mode mode) +{ + switch (mode) { + case ST_LPS22DF_BYPASS: + case ST_LPS22DF_STREAM: + break; + default: + return -EINVAL; + } + + return st_lps22df_write_with_mask(hw, ST_LPS22DF_FIFO_CTRL_ADDR, + ST_LPS22DF_FIFO_MODE_MASK, mode); +} + +static int st_lps22df_update_fifo_watermark(struct st_lps22df_hw *hw, u8 val) +{ + int err; + + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_FIFO_WTM_ADDR, + ST_LPS22DF_FIFO_THS_MASK, val); + if (err < 0) + return err; + + hw->watermark = val; + + return 0; +} + +ssize_t st_lps22df_sysfs_set_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct st_lps22df_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err, watermark; + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if (watermark < 1 || watermark > ST_LPS22DF_MAX_FIFO_LENGTH) + return -EINVAL; + + err = st_lps22df_update_fifo_watermark(sensor->hw, watermark); + + return err < 0 ? err : count; +} + +static int st_lps22df_read_fifo(struct st_lps22df_hw *hw, s64 delta_ts) +{ + u8 iio_buff[ALIGN(sizeof(u32) + sizeof(s64), sizeof(s64))]; + u8 buff[ST_LPS22DF_RX_MAX_LENGTH]; + int err, i, read_len; + __le16 fifo_status; + + err = hw->tf->read(hw->dev, ST_LPS22DF_FIFO_STATUS1_ADDR, + sizeof(fifo_status), (u8 *)&fifo_status); + if (err < 0) + return err; + + read_len = (le16_to_cpu(fifo_status) & ST_LPS22DF_FIFO_SRC_DIFF_MASK) * + ST_LPS22DF_PRESS_SAMPLE_LEN; + if (!read_len) + return 0; + + err = hw->tf->read(hw->dev, ST_LPS22DF_FIFO_DATA_OUT_PRESS_XL_ADDR, + read_len, buff); + if (err < 0) + return err; + + for (i = 0; i < read_len; i += ST_LPS22DF_PRESS_SAMPLE_LEN) { + memcpy(iio_buff, buff + i, ST_LPS22DF_PRESS_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS22DF_PRESS], + iio_buff, hw->ts); + hw->ts += delta_ts; + } + + hw->last_fifo_ts = hw->ts; + + return read_len; +} + +ssize_t st_lps22df_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct st_lps22df_sensor *sensor = iio_priv(indio_dev); + struct st_lps22df_hw *hw = sensor->hw; + int len, dir; + s64 fts; + + mutex_lock(&hw->fifo_lock); + len = st_lps22df_read_fifo(hw, hw->delta_ts); + hw->ts = st_lps22df_get_time_ns(hw); + hw->ts_irq = hw->ts; + + /* flush event timestamp must match with last sample pushed in fifo */ + if (len) + fts = hw->ts; + else + fts = hw->last_fifo_ts; + + mutex_unlock(&hw->fifo_lock); + + dir = len > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + iio_push_event(indio_dev, + IIO_UNMOD_EVENT_CODE(IIO_PRESSURE, -1, + IIO_EV_TYPE_FIFO_FLUSH, dir), + fts); + + return size; +} + +static irqreturn_t st_lps22df_irq_handler(int irq, void *private) +{ + struct st_lps22df_hw *hw = private; + s64 delta_ts, ts = st_lps22df_get_time_ns(hw); + + delta_ts = div_s64((ts - hw->ts_irq), hw->watermark); + if (hw->odr >= 50) + hw->delta_ts = st_lps22df_ewma(hw->delta_ts, delta_ts, + ST_LPS22DF_EWMA_LEVEL); + else + hw->delta_ts = delta_ts; + + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lps22df_irq_thread(int irq, void *private) +{ + struct st_lps22df_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_lps22df_read_fifo(hw, hw->delta_ts); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_lps22df_buffer_preenable(struct iio_dev *indio_dev) +{ + struct st_lps22df_sensor *sensor = iio_priv(indio_dev); + struct st_lps22df_hw *hw = sensor->hw; + int err; + + err = st_lps22df_set_fifo_mode(sensor->hw, ST_LPS22DF_STREAM); + if (err < 0) + return err; + + err = st_lps22df_update_fifo_watermark(hw, hw->watermark); + if (err < 0) + return err; + + err = st_lps22df_write_with_mask(sensor->hw, ST_LPS22DF_CTRL4_ADDR, + ST_LPS22DF_INT_F_WTM_MASK, true); + if (err < 0) + return err; + + err = st_lps22df_set_enable(sensor, true); + if (err < 0) + return err; + + hw->delta_ts = div_s64(1000000000UL, hw->odr); + hw->ts = st_lps22df_get_time_ns(hw); + hw->ts_irq = hw->ts; + + return 0; +} + +static int st_lps22df_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct st_lps22df_sensor *sensor = iio_priv(indio_dev); + int err; + + err = st_lps22df_set_fifo_mode(sensor->hw, ST_LPS22DF_BYPASS); + if (err < 0) + return err; + + err = st_lps22df_write_with_mask(sensor->hw, ST_LPS22DF_CTRL4_ADDR, + ST_LPS22DF_INT_F_WTM_MASK, false); + if (err < 0) + return err; + + return st_lps22df_set_enable(sensor, false); +} + +static const struct iio_buffer_setup_ops st_lps22df_buffer_ops = { + .preenable = st_lps22df_buffer_preenable, + .postdisable = st_lps22df_buffer_postdisable, +}; + +int st_lps22df_allocate_buffers(struct st_lps22df_hw *hw) +{ +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + unsigned long irq_type; + u8 int_active = 0; + int err; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); + + switch (irq_type) { + case IRQF_TRIGGER_HIGH: + case IRQF_TRIGGER_RISING: + int_active = 0; + break; + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_FALLING: + int_active = 1; + break; + default: + dev_info(hw->dev, "mode %lx unsupported\n", irq_type); + + return -EINVAL; + } + + /* int pin active low */ + if (device_property_read_bool(hw->dev, "int-active-low")) { + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_CTRL3_ADDR, + ST_LPS22DF_INT_H_L_MASK, 1); + if (err < 0) + return err; + } + + /* int pin open drain configuration */ + if (device_property_read_bool(hw->dev, "int-open-drain")) { + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_CTRL3_ADDR, + ST_LPS22DF_PP_OD_MASK, 1); + if (err < 0) + return err; + } + + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_CTRL3_ADDR, + ST_LPS22DF_INT_H_L_MASK, + int_active); + if (err < 0) + return err; + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lps22df_irq_handler, + st_lps22df_irq_thread, + irq_type | IRQF_ONESHOT, + "lps22df", hw); + if (err) + return err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, + hw->iio_devs[ST_LPS22DF_PRESS], + INDIO_BUFFER_SOFTWARE, + &st_lps22df_buffer_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[ST_LPS22DF_PRESS], buffer); + hw->iio_devs[ST_LPS22DF_PRESS]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[ST_LPS22DF_PRESS]->setup_ops = &st_lps22df_buffer_ops; +#endif /* LINUX_VERSION_CODE */ + + return 0; +} diff --git a/drivers/iio/stm/pressure/st_lps22df_core.c b/drivers/iio/stm/pressure/st_lps22df_core.c new file mode 100644 index 000000000000..c655d24b2de0 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22df_core.c @@ -0,0 +1,461 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22df driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lps22df.h" + +struct st_lps22df_odr_table_t { + u8 addr; + u8 mask; + u8 odr_avl[ST_LPS22DF_ODR_LIST_NUM]; +}; + +const static struct st_lps22df_odr_table_t st_lps22df_odr_table = { + .addr = ST_LPS22DF_CTRL_REG1_ADDR, + .mask = ST_LPS22DF_ODR_MASK, + .odr_avl = { 0, 1, 4, 10, 25, 50, 75, 100, 200 }, +}; + +const struct iio_event_spec st_lps22df_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lps22df_press_channels[] = { + { + .type = IIO_PRESSURE, + .address = ST_LPS22DF_PRESS_OUT_XL_ADDR, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 24, + .storagebits = 32, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_PRESSURE, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps22df_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lps22df_temp_channels[] = { + { + .type = IIO_TEMP, + .address = ST_LPS22DF_TEMP_OUT_L_ADDR, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, +}; + +int st_lps22df_write_with_mask(struct st_lps22df_hw *hw, u8 addr, u8 mask, + u8 val) +{ + int err; + u8 data; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) + goto unlock; + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); +unlock: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_lps22df_check_whoami(struct st_lps22df_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_LPS22DF_WHO_AM_I_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read Who-Am-I register\n"); + + return err; + } + + if (data != ST_LPS22DF_WHO_AM_I_VAL) { + dev_err(hw->dev, "Who-Am-I value not valid (%x)\n", data); + + return -ENODEV; + } + + return 0; +} + +static int st_lps22df_get_odr(struct st_lps22df_sensor *sensor, u8 odr) +{ + int i; + + for (i = 0; i < ST_LPS22DF_ODR_LIST_NUM; i++) { + if (st_lps22df_odr_table.odr_avl[i] == odr) + break; + } + + return i == ST_LPS22DF_ODR_LIST_NUM ? -EINVAL : i; +} + +int st_lps22df_set_enable(struct st_lps22df_sensor *sensor, bool enable) +{ + struct st_lps22df_hw *hw = sensor->hw; + u32 max_odr = enable ? sensor->odr : 0; + int i; + + for (i = 0; i < ST_LPS22DF_SENSORS_NUMB; i++) { + if (sensor->type == i) + continue; + + if (hw->enable_mask & BIT(i)) { + struct st_lps22df_sensor *temp; + + temp = iio_priv(hw->iio_devs[i]); + max_odr = max_t(u32, max_odr, temp->odr); + } + } + + if (max_odr != hw->odr) { + int err, ret; + + ret = st_lps22df_get_odr(sensor, max_odr); + if (ret < 0) + return ret; + + err = st_lps22df_write_with_mask(hw, st_lps22df_odr_table.addr, + st_lps22df_odr_table.mask, + ret); + if (err < 0) + return err; + + hw->odr = max_odr; + } + + if (enable) + hw->enable_mask |= BIT(sensor->type); + else + hw->enable_mask &= ~BIT(sensor->type); + + return 0; +} + +int st_lps22df_init_sensors(struct st_lps22df_hw *hw) +{ + int err; + + /* reboot memory content */ + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_CTRL_REG2_ADDR, + ST_LPS22DF_BOOT_MASK, 1); + if (err < 0) + return err; + + usleep_range(8000, 10000); + + /* soft reset the device on power on */ + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_CTRL_REG2_ADDR, + ST_LPS22DF_SWRESET_MASK, 1); + if (err < 0) + return err; + + usleep_range(100, 200); + + /* enable latched interrupt mode */ + err = st_lps22df_write_with_mask(hw, ST_LPS22DF_INTERRUPT_CFG_ADDR, + ST_LPS22DF_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable BDU */ + return st_lps22df_write_with_mask(hw, ST_LPS22DF_CTRL_REG2_ADDR, + ST_LPS22DF_BDU_MASK, 1); +} + +static ssize_t +st_lps22df_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 1; i < ST_LPS22DF_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lps22df_odr_table.odr_avl[i]); + } + + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lps22df_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lps22df_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->hw->watermark); +} + +static ssize_t +st_lps22df_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LPS22DF_MAX_FIFO_LENGTH); +} + +static int st_lps22df_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lps22df_sensor *sensor = iio_priv(indio_dev); + struct st_lps22df_hw *hw = sensor->hw; + int ret, delay; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + u8 data[4] = {}; + u8 len; + + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + + ret = st_lps22df_set_enable(sensor, true); + if (ret < 0) + goto unlock; + + /* wait at least 10% more than one odr */ + delay = 1100000 / sensor->odr; + usleep_range(delay, 2 * delay); + len = ch->scan_type.realbits >> 3; + ret = hw->tf->read(hw->dev, ch->address, len, data); + if (ret < 0) + goto unlock; + + if (sensor->type == ST_LPS22DF_PRESS) + *val = (s32)get_unaligned_le32(data); + else if (sensor->type == ST_LPS22DF_TEMP) + *val = (s16)get_unaligned_le16(data); + +unlock: + ret = st_lps22df_set_enable(sensor, false); + iio_device_release_direct_mode(indio_dev); + + ret = ret < 0 ? ret : IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + switch (ch->type) { + case IIO_TEMP: + *val = 1000; + *val2 = sensor->gain; + ret = IIO_VAL_FRACTIONAL; + break; + case IIO_PRESSURE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + ret = -ENODEV; + break; + } + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lps22df_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int val, int val2, long mask) +{ + struct st_lps22df_sensor *sensor = iio_priv(indio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + ret = st_lps22df_get_odr(sensor, val); + if (ret > 0) + sensor->odr = val; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lps22df_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_lps22df_sysfs_get_hwfifo_watermark, + st_lps22df_sysfs_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lps22df_sysfs_get_hwfifo_watermark_max, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, + st_lps22df_sysfs_flush_fifo, 0); + +static struct attribute *st_lps22df_press_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static struct attribute *st_lps22df_temp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lps22df_press_attribute_group = { + .attrs = st_lps22df_press_attributes, +}; +static const struct attribute_group st_lps22df_temp_attribute_group = { + .attrs = st_lps22df_temp_attributes, +}; + +static const struct iio_info st_lps22df_press_info = { + .attrs = &st_lps22df_press_attribute_group, + .read_raw = st_lps22df_read_raw, + .write_raw = st_lps22df_write_raw, +}; + +static const struct iio_info st_lps22df_temp_info = { + .attrs = &st_lps22df_temp_attribute_group, + .read_raw = st_lps22df_read_raw, + .write_raw = st_lps22df_write_raw, +}; + +int st_lps22df_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps22df_transfer_function *tf_ops) +{ + struct st_lps22df_sensor *sensor; + struct st_lps22df_hw *hw; + struct iio_dev *iio_dev; + int err, i; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + hw->dev = dev; + hw->tf = tf_ops; + hw->irq = irq; + + /* set initial watermark */ + hw->watermark = 1; + + mutex_init(&hw->lock); + mutex_init(&hw->fifo_lock); + + err = st_lps22df_check_whoami(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LPS22DF_SENSORS_NUMB; i++) { + iio_dev = devm_iio_device_alloc(dev, sizeof(*sensor)); + if (!iio_dev) + return -ENOMEM; + + hw->iio_devs[i] = iio_dev; + sensor = iio_priv(iio_dev); + sensor->hw = hw; + sensor->type = i; + sensor->odr = 1; + + switch (i) { + case ST_LPS22DF_PRESS: + sensor->gain = ST_LPS22DF_PRESS_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", name); + iio_dev->channels = st_lps22df_press_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps22df_press_channels); + iio_dev->info = &st_lps22df_press_info; + break; + case ST_LPS22DF_TEMP: + sensor->gain = ST_LPS22DF_TEMP_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", name); + iio_dev->channels = st_lps22df_temp_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps22df_temp_channels); + iio_dev->info = &st_lps22df_temp_info; + break; + default: + return -EINVAL; + } + + iio_dev->name = sensor->name; + iio_dev->modes = INDIO_DIRECT_MODE; + } + + err = st_lps22df_init_sensors(hw); + if (err < 0) + return err; + + if (irq > 0) { + err = st_lps22df_allocate_buffers(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LPS22DF_SENSORS_NUMB; i++) { + err = devm_iio_device_register(dev, hw->iio_devs[i]); + if (err) + return err; + } + return 0; +} +EXPORT_SYMBOL(st_lps22df_common_probe); + +MODULE_DESCRIPTION("STMicroelectronics lps22df driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22df_i2c.c b/drivers/iio/stm/pressure/st_lps22df_i2c.c new file mode 100644 index 000000000000..ecab55b11bc0 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22df_i2c.c @@ -0,0 +1,105 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22df i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include + +#include "st_lps22df.h" + +static int st_lps22df_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_lps22df_hw *hw = dev_get_drvdata(dev); + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + int ret; + + if (len >= ST_LPS22DF_RX_MAX_LENGTH) + return -ENOMEM; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = hw->tb.rx_buf; + + ret = i2c_transfer(client->adapter, msg, 2); + if (ret < 0) + return ret; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return 0; +} + +static int st_lps22df_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_lps22df_hw *hw = dev_get_drvdata(dev); + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + int ret; + + if (len >= ST_LPS22DF_TX_MAX_LENGTH) + return -ENOMEM; + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = hw->tb.tx_buf; + + ret = i2c_transfer(client->adapter, &msg, 1); + + return ret < 0 ? ret : 0; +} + +static const struct st_lps22df_transfer_function st_lps22df_tf_i2c = { + .write = st_lps22df_i2c_write, + .read = st_lps22df_i2c_read, +}; + +static int st_lps22df_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_lps22df_common_probe(&client->dev, client->irq, client->name, + &st_lps22df_tf_i2c); +} + +static const struct i2c_device_id st_lps22df_ids[] = { + { "lps22df" }, + { "lps28dfw" }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_lps22df_ids); + +static const struct of_device_id st_lps22df_id_table[] = { + { .compatible = "st,lps22df" }, + { .compatible = "st,lps28dfw" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps22df_id_table); + +static struct i2c_driver st_lps22df_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps22df_i2c", + .of_match_table = of_match_ptr(st_lps22df_id_table), + }, + .probe = st_lps22df_i2c_probe, + .id_table = st_lps22df_ids, +}; +module_i2c_driver(st_lps22df_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics lps22df i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22df_spi.c b/drivers/iio/stm/pressure/st_lps22df_spi.c new file mode 100644 index 000000000000..d10308fea4c7 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22df_spi.c @@ -0,0 +1,102 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22df spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include + +#include "st_lps22df.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lps22df_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_lps22df_hw *hw = spi_get_drvdata(spi); + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + int err; + + if (len >= ST_LPS22DF_RX_MAX_LENGTH) + return -ENOMEM; + + hw->tb.tx_buf[0] = addr | ST_SENSORS_SPI_READ; + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err) + return err; + + memcpy(data, hw->tb.rx_buf, len); + + return err; +} + +static int st_lps22df_spi_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_lps22df_hw *hw; + struct spi_device *spi; + + if (len >= ST_LPS22DF_TX_MAX_LENGTH) + return -ENOMEM; + + spi = to_spi_device(dev); + hw = spi_get_drvdata(spi); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_lps22df_transfer_function st_lps22df_tf_spi = { + .write = st_lps22df_spi_write, + .read = st_lps22df_spi_read, +}; + +static int st_lps22df_spi_probe(struct spi_device *spi) +{ + return st_lps22df_common_probe(&spi->dev, spi->irq, spi->modalias, + &st_lps22df_tf_spi); +} + +static const struct spi_device_id st_lps22df_ids[] = { + { "lps22df" }, + { "lps28dfw" }, + {} +}; +MODULE_DEVICE_TABLE(spi, st_lps22df_ids); + +static const struct of_device_id st_lps22df_id_table[] = { + { .compatible = "st,lps22df" }, + { .compatible = "st,lps28dfw" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps22df_id_table); + +static struct spi_driver st_lps22df_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps22df_spi", + .of_match_table = of_match_ptr(st_lps22df_id_table), + }, + .probe = st_lps22df_spi_probe, + .id_table = st_lps22df_ids, +}; +module_spi_driver(st_lps22df_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics lps22df spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22hb.h b/drivers/iio/stm/pressure/st_lps22hb.h new file mode 100644 index 000000000000..ef89dc541e3f --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hb.h @@ -0,0 +1,88 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lps22hb driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#ifndef __ST_LPS22HB_H +#define __ST_LPS22HB_H + +#include +#include +#include +#include + +#define ST_LPS22HB_MAX_FIFO_LENGTH 31 + +#define ST_LPS22HB_CTRL3_ADDR 0x12 + +enum st_lps22hb_sensor_type { + ST_LPS22HB_PRESS = 0, + ST_LPS22HB_TEMP, + ST_LPS22HB_SENSORS_NUMB, +}; + +enum st_lps22hb_fifo_mode { + ST_LPS22HB_BYPASS = 0x0, + ST_LPS22HB_STREAM = 0x6, +}; + +#define ST_LPS22HB_TX_MAX_LENGTH 8 +#define ST_LPS22HB_RX_MAX_LENGTH 192 + +struct st_lps22hb_transfer_buffer { + u8 rx_buf[ST_LPS22HB_RX_MAX_LENGTH]; + u8 tx_buf[ST_LPS22HB_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_lps22hb_transfer_function { + int (*write)(struct device *dev, u8 addr, int len, u8 *data); + int (*read)(struct device *dev, u8 addr, int len, u8 *data); +}; + +struct st_lps22hb_hw { + struct device *dev; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + u8 watermark; + + struct iio_dev *iio_devs[ST_LPS22HB_SENSORS_NUMB]; + u8 enable_mask; + u8 odr; + + s64 delta_ts; + s64 ts_irq; + s64 ts; + + const struct st_lps22hb_transfer_function *tf; + struct st_lps22hb_transfer_buffer tb; +}; + +struct st_lps22hb_sensor { + struct st_lps22hb_hw *hw; + enum st_lps22hb_sensor_type type; + char name[32]; + + u32 gain; + u8 odr; +}; + +int st_lps22hb_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps22hb_transfer_function *tf_ops); +int st_lps22hb_write_with_mask(struct st_lps22hb_hw *hw, u8 addr, u8 mask, + u8 data); +int st_lps22hb_allocate_buffers(struct st_lps22hb_hw *hw); +int st_lps22hb_set_enable(struct st_lps22hb_sensor *sensor, bool enable); +ssize_t st_lps22hb_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count); +ssize_t st_lps22hb_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); + +#endif /* __ST_LPS22HB_H */ diff --git a/drivers/iio/stm/pressure/st_lps22hb_buffer.c b/drivers/iio/stm/pressure/st_lps22hb_buffer.c new file mode 100644 index 000000000000..917e8e10263e --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hb_buffer.c @@ -0,0 +1,288 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hb buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lps22hb.h" + +#define ST_LPS22HB_FIFO_CTRL_ADDR 0x14 +#define ST_LPS22HB_FIFO_THS_MASK 0x1f +#define ST_LPS22HB_FIFO_MODE_MASK 0xe0 +#define ST_LPS22HB_INT_FTH_MASK 0x10 + +#define ST_LPS22HB_FIFO_SRC_ADDR 0x26 +#define ST_LPS22HB_FIFO_SRC_DIFF_MASK 0x1f + +#define ST_LPS22HB_PRESS_OUT_XL_ADDR 0x28 + +#define ST_LPS22HB_PRESS_SAMPLE_LEN 3 +#define ST_LPS22HB_TEMP_SAMPLE_LEN 2 +#define ST_LPS22HB_FIFO_SAMPLE_LEN (ST_LPS22HB_PRESS_SAMPLE_LEN + \ + ST_LPS22HB_TEMP_SAMPLE_LEN) + +static inline s64 st_lps22hb_get_time_ns(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +#define ST_LPS22HB_EWMA_LEVEL 96 +#define ST_LPS22HB_EWMA_DIV 128 +static inline s64 st_lps22hb_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LPS22HB_EWMA_DIV - weight) * diff, + ST_LPS22HB_EWMA_DIV); + + return old + incr; +} + +static int st_lps22hb_set_fifo_mode(struct st_lps22hb_hw *hw, + enum st_lps22hb_fifo_mode mode) +{ + switch (mode) { + case ST_LPS22HB_BYPASS: + case ST_LPS22HB_STREAM: + break; + default: + return -EINVAL; + } + return st_lps22hb_write_with_mask(hw, ST_LPS22HB_FIFO_CTRL_ADDR, + ST_LPS22HB_FIFO_MODE_MASK, mode); +} + +static int st_lps22hb_update_fifo_watermark(struct st_lps22hb_hw *hw, u8 val) +{ + int err; + + err = st_lps22hb_write_with_mask(hw, ST_LPS22HB_FIFO_CTRL_ADDR, + ST_LPS22HB_FIFO_THS_MASK, val); + if (err < 0) + return err; + + hw->watermark = val; + + return 0; +} + +ssize_t st_lps22hb_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + struct st_lps22hb_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err, watermark; + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if (watermark < 1 || watermark > ST_LPS22HB_MAX_FIFO_LENGTH) + return -EINVAL; + + err = st_lps22hb_update_fifo_watermark(sensor->hw, watermark); + + return err < 0 ? err : count; +} + +static int st_lps22hb_read_fifo(struct st_lps22hb_hw *hw) +{ + u8 iio_buff[ALIGN(sizeof(u32) + sizeof(s64), sizeof(s64))]; + u8 status, buff[ST_LPS22HB_RX_MAX_LENGTH]; + int err, i, read_len; + + err = hw->tf->read(hw->dev, ST_LPS22HB_FIFO_SRC_ADDR, + sizeof(status), &status); + if (err < 0) + return err; + + read_len = (status & ST_LPS22HB_FIFO_SRC_DIFF_MASK) * + ST_LPS22HB_FIFO_SAMPLE_LEN; + if (!read_len) + return 0; + + err = hw->tf->read(hw->dev, ST_LPS22HB_PRESS_OUT_XL_ADDR, + read_len, buff); + if (err < 0) + return err; + + for (i = 0; i < read_len; i += ST_LPS22HB_FIFO_SAMPLE_LEN) { + /* press sample */ + memcpy(iio_buff, buff + i, ST_LPS22HB_PRESS_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS22HB_PRESS], + iio_buff, hw->ts); + /* temp sample */ + memcpy(iio_buff, buff + i + ST_LPS22HB_PRESS_SAMPLE_LEN, + ST_LPS22HB_TEMP_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS22HB_TEMP], + iio_buff, hw->ts); + hw->ts += hw->delta_ts; + } + + return read_len; +} + +ssize_t st_lps22hb_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct st_lps22hb_sensor *sensor = iio_priv(indio_dev); + struct st_lps22hb_hw *hw = sensor->hw; + u64 type, event; + int len; + + mutex_lock(&indio_dev->mlock); + if (!iio_buffer_enabled(indio_dev)) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + mutex_lock(&hw->fifo_lock); + len = st_lps22hb_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + type = len > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + if (sensor->type == ST_LPS22HB_PRESS) + event = IIO_UNMOD_EVENT_CODE(IIO_PRESSURE, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + else + event = IIO_UNMOD_EVENT_CODE(IIO_TEMP, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(indio_dev, event, st_lps22hb_get_time_ns(indio_dev)); + mutex_unlock(&indio_dev->mlock); + + return size; +} + + +static irqreturn_t st_lps22hb_irq_handler(int irq, void *private) +{ + struct st_lps22hb_hw *hw = private; + s64 delta_ts, ts; + + ts = st_lps22hb_get_time_ns(hw->iio_devs[ST_LPS22HB_PRESS]); + delta_ts = div_s64((ts - hw->ts_irq), hw->watermark); + if (hw->odr >= 50) + hw->delta_ts = st_lps22hb_ewma(hw->delta_ts, delta_ts, + ST_LPS22HB_EWMA_LEVEL); + else + hw->delta_ts = delta_ts; + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lps22hb_irq_thread(int irq, void *private) +{ + struct st_lps22hb_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_lps22hb_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_lps22hb_buffer_preenable(struct iio_dev *indio_dev) +{ + struct st_lps22hb_sensor *sensor = iio_priv(indio_dev); + struct st_lps22hb_hw *hw = sensor->hw; + int err; + + err = st_lps22hb_set_fifo_mode(sensor->hw, ST_LPS22HB_STREAM); + if (err < 0) + return err; + + err = st_lps22hb_update_fifo_watermark(hw, hw->watermark); + if (err < 0) + return err; + + err = st_lps22hb_write_with_mask(sensor->hw, ST_LPS22HB_CTRL3_ADDR, + ST_LPS22HB_INT_FTH_MASK, true); + if (err < 0) + return err; + + err = st_lps22hb_set_enable(sensor, true); + if (err < 0) + return err; + + hw->delta_ts = div_s64(1000000000UL, hw->odr); + hw->ts = st_lps22hb_get_time_ns(indio_dev); + hw->ts_irq = hw->ts; + + return 0; +} + +static int st_lps22hb_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct st_lps22hb_sensor *sensor = iio_priv(indio_dev); + int err; + + err = st_lps22hb_set_fifo_mode(sensor->hw, ST_LPS22HB_BYPASS); + if (err < 0) + return err; + + err = st_lps22hb_write_with_mask(sensor->hw, ST_LPS22HB_CTRL3_ADDR, + ST_LPS22HB_INT_FTH_MASK, false); + if (err < 0) + return err; + + return st_lps22hb_set_enable(sensor, false); +} + +static const struct iio_buffer_setup_ops st_lps22hb_buffer_ops = { + .preenable = st_lps22hb_buffer_preenable, + .postdisable = st_lps22hb_buffer_postdisable, +}; + +int st_lps22hb_allocate_buffers(struct st_lps22hb_hw *hw) +{ +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + int err, i; + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lps22hb_irq_handler, + st_lps22hb_irq_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + "lps22hb", hw); + if (err) + return err; + + for (i = 0; i < ST_LPS22HB_SENSORS_NUMB; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + INDIO_BUFFER_SOFTWARE, + &st_lps22hb_buffer_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_lps22hb_buffer_ops; +#endif /* LINUX_VERSION_CODE */ + + } + + return 0; +} diff --git a/drivers/iio/stm/pressure/st_lps22hb_core.c b/drivers/iio/stm/pressure/st_lps22hb_core.c new file mode 100644 index 000000000000..6984fc94e40f --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hb_core.c @@ -0,0 +1,496 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hb driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lps22hb.h" + +#define ST_LPS22HB_WHO_AM_I_ADDR 0x0f +#define ST_LPS22HB_WHO_AM_I_DEF 0xb1 + +#define ST_LPS22HB_CTRL1_ADDR 0x10 +#define ST_LPS22HB_BDU_MASK 0x02 +#define ST_LPS22HB_CTRL2_ADDR 0x11 +#define ST_LPS22HB_SOFT_RESET_MASK 0x04 +#define ST_LPS22HB_FIFO_ENABLE_MASK 0x40 + +#define ST_LPS22HB_LIR_ADDR 0x0b +#define ST_LPS22HB_LIR_MASK 0x04 + +#define ST_LPS22HB_PRESS_FS_AVL_GAIN (1000000000UL / 4096UL) +#define ST_LPS22HB_TEMP_FS_AVL_GAIN 100 + +#define ST_LPS22HB_ODR_LIST_NUM 6 +struct st_lps22hb_odr_table_t { + u8 addr; + u8 mask; + u8 odr_avl[ST_LPS22HB_ODR_LIST_NUM]; +}; + +const static struct st_lps22hb_odr_table_t st_lps22hb_odr_table = { + .addr = 0x10, + .mask = 0x70, + .odr_avl = { 0, 1, 10, 25, 50, 75 }, +}; + +const struct iio_event_spec st_lps22hb_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lps22hb_press_channels[] = { + { + .type = IIO_PRESSURE, + .address = 0x28, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 24, + .storagebits = 32, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_PRESSURE, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps22hb_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lps22hb_temp_channels[] = { + { + .type = IIO_TEMP, + .address = 0x2b, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_TEMP, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps22hb_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +int st_lps22hb_write_with_mask(struct st_lps22hb_hw *hw, u8 addr, u8 mask, + u8 val) +{ + int err; + u8 data; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) + goto unlock; + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); +unlock: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_lps22hb_check_whoami(struct st_lps22hb_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_LPS22HB_WHO_AM_I_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + if (data != ST_LPS22HB_WHO_AM_I_DEF) { + dev_err(hw->dev, "Who-Am-I value not valid.\n"); + return -ENODEV; + } + return 0; +} + +static int st_lps22hb_get_odr(struct st_lps22hb_sensor *sensor, u8 odr) +{ + int i; + + for (i = 0; i < ST_LPS22HB_ODR_LIST_NUM; i++) { + if (st_lps22hb_odr_table.odr_avl[i] == odr) + break; + } + return i == ST_LPS22HB_ODR_LIST_NUM ? -EINVAL : i; +} + +int st_lps22hb_set_enable(struct st_lps22hb_sensor *sensor, bool enable) +{ + struct st_lps22hb_hw *hw = sensor->hw; + u32 max_odr = enable ? sensor->odr : 0; + int i; + + for (i = 0; i < ST_LPS22HB_SENSORS_NUMB; i++) { + if (sensor->type == i) + continue; + + if (hw->enable_mask & BIT(i)) { + struct st_lps22hb_sensor *temp; + + temp = iio_priv(hw->iio_devs[i]); + max_odr = max_t(u32, max_odr, temp->odr); + } + } + + if (max_odr != hw->odr) { + int err, ret; + + ret = st_lps22hb_get_odr(sensor, max_odr); + if (ret < 0) + return ret; + + err = st_lps22hb_write_with_mask(hw, st_lps22hb_odr_table.addr, + st_lps22hb_odr_table.mask, ret); + if (err < 0) + return err; + + hw->odr = max_odr; + } + + if (enable) + hw->enable_mask |= BIT(sensor->type); + else + hw->enable_mask &= ~BIT(sensor->type); + + return 0; +} + +int st_lps22hb_init_sensors(struct st_lps22hb_hw *hw) +{ + int err; + + /* soft reset the device on power on. */ + err = st_lps22hb_write_with_mask(hw, ST_LPS22HB_CTRL2_ADDR, + ST_LPS22HB_SOFT_RESET_MASK, 1); + if (err < 0) + return err; + + msleep(200); + + /* enable latched interrupt mode */ + err = st_lps22hb_write_with_mask(hw, ST_LPS22HB_LIR_ADDR, + ST_LPS22HB_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable FIFO */ + err = st_lps22hb_write_with_mask(hw, ST_LPS22HB_CTRL2_ADDR, + ST_LPS22HB_FIFO_ENABLE_MASK, 1); + if (err < 0) + return err; + + /* enable BDU */ + return st_lps22hb_write_with_mask(hw, ST_LPS22HB_CTRL1_ADDR, + ST_LPS22HB_BDU_MASK, 1); +} + +static ssize_t +st_lps22hb_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 1; i < ST_LPS22HB_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lps22hb_odr_table.odr_avl[i]); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lps22hb_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lps22hb_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->hw->watermark); +} + +static ssize_t +st_lps22hb_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +static ssize_t +st_lps22hb_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LPS22HB_MAX_FIFO_LENGTH); +} + +static int st_lps22hb_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lps22hb_sensor *sensor = iio_priv(indio_dev); + struct st_lps22hb_hw *hw = sensor->hw; + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + u8 len = ch->scan_type.realbits / 8; + u8 data[4] = {}; + + mutex_lock(&indio_dev->mlock); + if (iio_buffer_enabled(indio_dev)) { + mutex_unlock(&indio_dev->mlock); + ret = -EBUSY; + break; + } + + ret = st_lps22hb_set_enable(sensor, true); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + ret = -EBUSY; + break; + } + + msleep(40); + ret = hw->tf->read(hw->dev, ch->address, len, data); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + return ret; + } + + if (sensor->type == ST_LPS22HB_PRESS) + *val = (s32)get_unaligned_le32(data); + else if (sensor->type == ST_LPS22HB_TEMP) + *val = (s16)get_unaligned_le16(data); + + ret = st_lps22hb_set_enable(sensor, false); + mutex_unlock(&indio_dev->mlock); + + if (ret < 0) + return ret; + + ret = IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + switch (ch->type) { + case IIO_TEMP: + *val = 1000; + *val2 = sensor->gain; + ret = IIO_VAL_FRACTIONAL; + break; + case IIO_PRESSURE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + ret = -ENODEV; + break; + } + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static int st_lps22hb_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int val, int val2, long mask) +{ + struct st_lps22hb_sensor *sensor = iio_priv(indio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + ret = st_lps22hb_get_odr(sensor, val); + if (ret > 0) + sensor->odr = val; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lps22hb_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, + st_lps22hb_sysfs_get_hwfifo_watermark, + st_lps22hb_sysfs_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, + st_lps22hb_sysfs_get_hwfifo_watermark_min, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, + st_lps22hb_sysfs_get_hwfifo_watermark_max, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, + st_lps22hb_sysfs_flush_fifo, 0); + +static struct attribute *st_lps22hb_press_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static struct attribute *st_lps22hb_temp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lps22hb_press_attribute_group = { + .attrs = st_lps22hb_press_attributes, +}; +static const struct attribute_group st_lps22hb_temp_attribute_group = { + .attrs = st_lps22hb_temp_attributes, +}; + +static const struct iio_info st_lps22hb_press_info = { + .attrs = &st_lps22hb_press_attribute_group, + .read_raw = st_lps22hb_read_raw, + .write_raw = st_lps22hb_write_raw, +}; + +static const struct iio_info st_lps22hb_temp_info = { + .attrs = &st_lps22hb_temp_attribute_group, + .read_raw = st_lps22hb_read_raw, + .write_raw = st_lps22hb_write_raw, +}; + +int st_lps22hb_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps22hb_transfer_function *tf_ops) +{ + struct st_lps22hb_sensor *sensor; + struct st_lps22hb_hw *hw; + struct iio_dev *iio_dev; + int err, i; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + hw->dev = dev; + hw->tf = tf_ops; + hw->irq = irq; + /* set initial watermark */ + hw->watermark = 1; + + mutex_init(&hw->lock); + mutex_init(&hw->fifo_lock); + + err = st_lps22hb_check_whoami(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LPS22HB_SENSORS_NUMB; i++) { + iio_dev = devm_iio_device_alloc(dev, sizeof(*sensor)); + if (!iio_dev) + return -ENOMEM; + + hw->iio_devs[i] = iio_dev; + sensor = iio_priv(iio_dev); + sensor->hw = hw; + sensor->type = i; + sensor->odr = 1; + + switch (i) { + case ST_LPS22HB_PRESS: + sensor->gain = ST_LPS22HB_PRESS_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", name); + iio_dev->channels = st_lps22hb_press_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps22hb_press_channels); + iio_dev->info = &st_lps22hb_press_info; + break; + case ST_LPS22HB_TEMP: + sensor->gain = ST_LPS22HB_TEMP_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", name); + iio_dev->channels = st_lps22hb_temp_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps22hb_temp_channels); + iio_dev->info = &st_lps22hb_temp_info; + break; + default: + return -EINVAL; + }; + iio_dev->name = sensor->name; + iio_dev->modes = INDIO_DIRECT_MODE; + } + + err = st_lps22hb_init_sensors(hw); + if (err < 0) + return err; + + if (irq > 0) { + err = st_lps22hb_allocate_buffers(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LPS22HB_SENSORS_NUMB; i++) { + err = devm_iio_device_register(dev, hw->iio_devs[i]); + if (err) + return err; + } + return 0; +} +EXPORT_SYMBOL(st_lps22hb_common_probe); + +MODULE_DESCRIPTION("STMicroelectronics lps22hb driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22hb_i2c.c b/drivers/iio/stm/pressure/st_lps22hb_i2c.c new file mode 100644 index 000000000000..8654ab73be9a --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hb_i2c.c @@ -0,0 +1,90 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hb i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include + +#include "st_lps22hb.h" + +static int st_lps22hb_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_lps22hb_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_lps22hb_transfer_function st_lps22hb_tf_i2c = { + .write = st_lps22hb_i2c_write, + .read = st_lps22hb_i2c_read, +}; + +static int st_lps22hb_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_lps22hb_common_probe(&client->dev, client->irq, client->name, + &st_lps22hb_tf_i2c); +} + +static const struct i2c_device_id st_lps22hb_ids[] = { + { "lps22hb" }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_lps22hb_ids); + +static const struct of_device_id st_lps22hb_id_table[] = { + { .compatible = "st,lps22hb" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps22hb_id_table); + +static struct i2c_driver st_lps22hb_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps22hb_i2c", + .of_match_table = of_match_ptr(st_lps22hb_id_table), + }, + .probe = st_lps22hb_i2c_probe, + .id_table = st_lps22hb_ids, +}; +module_i2c_driver(st_lps22hb_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics lps22hb i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22hb_spi.c b/drivers/iio/stm/pressure/st_lps22hb_spi.c new file mode 100644 index 000000000000..e6922634f899 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hb_spi.c @@ -0,0 +1,97 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hb spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include + +#include "st_lps22hb.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lps22hb_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_lps22hb_hw *hw = spi_get_drvdata(spi); + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + int err; + + hw->tb.tx_buf[0] = addr | ST_SENSORS_SPI_READ; + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err) + return err; + + memcpy(data, hw->tb.rx_buf, len); + + return len; +} + +static int st_lps22hb_spi_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_lps22hb_hw *hw; + struct spi_device *spi; + + if (len >= ST_LPS22HB_TX_MAX_LENGTH) + return -ENOMEM; + + spi = to_spi_device(dev); + hw = spi_get_drvdata(spi); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_lps22hb_transfer_function st_lps22hb_tf_spi = { + .write = st_lps22hb_spi_write, + .read = st_lps22hb_spi_read, +}; + +static int st_lps22hb_spi_probe(struct spi_device *spi) +{ + return st_lps22hb_common_probe(&spi->dev, spi->irq, spi->modalias, + &st_lps22hb_tf_spi); +} + +static const struct spi_device_id st_lps22hb_ids[] = { + { "lps22hb" }, + {} +}; +MODULE_DEVICE_TABLE(spi, st_lps22hb_ids); + +static const struct of_device_id st_lps22hb_id_table[] = { + { .compatible = "st,lps22hb" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps22hb_id_table); + +static struct spi_driver st_lps22hb_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps22hb_spi", + .of_match_table = of_match_ptr(st_lps22hb_id_table), + }, + .probe = st_lps22hb_spi_probe, + .id_table = st_lps22hb_ids, +}; +module_spi_driver(st_lps22hb_spi_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lps22hb spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22hh.h b/drivers/iio/stm/pressure/st_lps22hh.h new file mode 100644 index 000000000000..681c5fcec100 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hh.h @@ -0,0 +1,110 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lps22hh driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#ifndef __ST_LPS22HH_H +#define __ST_LPS22HH_H + +#include +#include +#include +#include + +#define ST_LPS22HH_MAX_FIFO_LENGTH 127 + +#define ST_LPS22HH_LIR_ADDR 0x0b +#define ST_LPS22HH_LIR_MASK 0x04 + +#define ST_LPS22HH_WHO_AM_I_ADDR 0x0f +#define ST_LPS22HH_WHO_AM_I_DEF 0xb3 + +#define ST_LPS22HH_CTRL1_ADDR 0x10 +#define ST_LPS22HH_BDU_MASK 0x02 + +#define ST_LPS22HH_CTRL2_ADDR 0x11 +#define ST_LPS22HH_LOW_NOISE_EN_MASK 0x02 +#define ST_LPS22HH_SOFT_RESET_MASK 0x04 +#define ST_LPS22HH_INT_ACTIVE_MASK 0x40 +#define ST_LPS22HH_BOOT_MASK 0x80 + +#define ST_LPS22HH_CTRL3_ADDR 0x12 +#define ST_LPS22HH_INT_FTH_MASK 0x10 + +enum st_lps22hh_sensor_type { + ST_LPS22HH_PRESS = 0, + ST_LPS22HH_TEMP, + ST_LPS22HH_SENSORS_NUMB, +}; + +enum st_lps22hh_fifo_mode { + ST_LPS22HH_BYPASS = 0x0, + ST_LPS22HH_STREAM = 0x2, +}; + +#define ST_LPS22HH_PRESS_SAMPLE_LEN 3 +#define ST_LPS22HH_TEMP_SAMPLE_LEN 2 +#define ST_LPS22HH_FIFO_SAMPLE_LEN (ST_LPS22HH_PRESS_SAMPLE_LEN + \ + ST_LPS22HH_TEMP_SAMPLE_LEN) + +#define ST_LPS22HH_TX_MAX_LENGTH 8 +#define ST_LPS22HH_RX_MAX_LENGTH (ST_LPS22HH_MAX_FIFO_LENGTH + 1) * \ + ST_LPS22HH_FIFO_SAMPLE_LEN + +struct st_lps22hh_transfer_buffer { + u8 rx_buf[ST_LPS22HH_RX_MAX_LENGTH]; + u8 tx_buf[ST_LPS22HH_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_lps22hh_transfer_function { + int (*write)(struct device *dev, u8 addr, int len, u8 *data); + int (*read)(struct device *dev, u8 addr, int len, u8 *data); +}; + +struct st_lps22hh_hw { + struct device *dev; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + u8 watermark; + + struct iio_dev *iio_devs[ST_LPS22HH_SENSORS_NUMB]; + u8 enable_mask; + u8 odr; + + s64 delta_ts; + s64 ts_irq; + s64 ts; + + const struct st_lps22hh_transfer_function *tf; + struct st_lps22hh_transfer_buffer tb; +}; + +struct st_lps22hh_sensor { + struct st_lps22hh_hw *hw; + enum st_lps22hh_sensor_type type; + char name[32]; + + u32 gain; + u8 odr; +}; + +int st_lps22hh_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps22hh_transfer_function *tf_ops); +int st_lps22hh_write_with_mask(struct st_lps22hh_hw *hw, u8 addr, u8 mask, + u8 data); +int st_lps22hh_allocate_buffers(struct st_lps22hh_hw *hw); +int st_lps22hh_set_enable(struct st_lps22hh_sensor *sensor, bool enable); +ssize_t st_lps22hh_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count); +ssize_t st_lps22hh_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); + +#endif /* __ST_LPS22HH_H */ diff --git a/drivers/iio/stm/pressure/st_lps22hh_buffer.c b/drivers/iio/stm/pressure/st_lps22hh_buffer.c new file mode 100644 index 000000000000..9ec34b05f9fe --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hh_buffer.c @@ -0,0 +1,309 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hh buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lps22hh.h" + +#define ST_LPS22HH_FIFO_CTRL_ADDR 0x13 +#define ST_LPS22HH_FIFO_MODE_MASK 0x03 + +#define ST_LPS22HH_FIFO_WTM_ADDR 0x14 +#define ST_LPS22HH_FIFO_THS_MASK 0x7f + +#define ST_LPS22HH_FIFO_SRC_ADDR 0x25 +#define ST_LPS22HH_FIFO_SRC_DIFF_MASK 0xff + +#define ST_LPS22HH_FIFO_DATA_OUT_PRESS_XL_ADDR 0x78 + +static inline s64 st_lps22hh_get_time_ns(struct st_lps22hh_hw *hw) +{ + return iio_get_time_ns(hw->iio_devs[ST_LPS22HH_PRESS]); +} + +static int st_lps22hh_set_fifo_mode(struct st_lps22hh_hw *hw, + enum st_lps22hh_fifo_mode mode) +{ + switch (mode) { + case ST_LPS22HH_BYPASS: + case ST_LPS22HH_STREAM: + break; + default: + return -EINVAL; + } + return st_lps22hh_write_with_mask(hw, ST_LPS22HH_FIFO_CTRL_ADDR, + ST_LPS22HH_FIFO_MODE_MASK, mode); +} + +static int st_lps22hh_update_fifo_watermark(struct st_lps22hh_hw *hw, u8 val) +{ + int err; + + err = st_lps22hh_write_with_mask(hw, ST_LPS22HH_FIFO_WTM_ADDR, + ST_LPS22HH_FIFO_THS_MASK, val); + if (err < 0) + return err; + + hw->watermark = val; + + return 0; +} + +ssize_t st_lps22hh_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + struct st_lps22hh_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err, watermark; + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if (watermark < 1 || watermark > ST_LPS22HH_MAX_FIFO_LENGTH) + return -EINVAL; + + err = st_lps22hh_update_fifo_watermark(sensor->hw, watermark); + + return err < 0 ? err : count; +} + +static int st_lps22hh_read_fifo(struct st_lps22hh_hw *hw, s64 delta_ts) +{ + u8 iio_buff[ALIGN(sizeof(u32) + sizeof(s64), sizeof(s64))]; + u8 status, buff[ST_LPS22HH_RX_MAX_LENGTH]; + int err, i, read_len; + + err = hw->tf->read(hw->dev, ST_LPS22HH_FIFO_SRC_ADDR, + sizeof(status), &status); + if (err < 0) + return err; + + read_len = (status & ST_LPS22HH_FIFO_SRC_DIFF_MASK) * + ST_LPS22HH_FIFO_SAMPLE_LEN; + if (!read_len) + return 0; + + err = hw->tf->read(hw->dev, ST_LPS22HH_FIFO_DATA_OUT_PRESS_XL_ADDR, + read_len, buff); + if (err < 0) + return err; + + for (i = 0; i < read_len; i += ST_LPS22HH_FIFO_SAMPLE_LEN) { + /* press sample */ + memcpy(iio_buff, buff + i, ST_LPS22HH_PRESS_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS22HH_PRESS], + iio_buff, hw->ts); + /* temp sample */ + memcpy(iio_buff, buff + i + ST_LPS22HH_PRESS_SAMPLE_LEN, + ST_LPS22HH_TEMP_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS22HH_TEMP], + iio_buff, hw->ts); + hw->ts += delta_ts; + } + + return read_len; +} + +ssize_t st_lps22hh_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct st_lps22hh_sensor *sensor = iio_priv(indio_dev); + struct st_lps22hh_hw *hw = sensor->hw; + u64 type, event; + int len; + + mutex_lock(&indio_dev->mlock); + if (!iio_buffer_enabled(indio_dev)) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + mutex_lock(&hw->fifo_lock); + len = st_lps22hh_read_fifo(hw, hw->delta_ts); + hw->ts = st_lps22hh_get_time_ns(hw); + hw->ts_irq = hw->ts; + mutex_unlock(&hw->fifo_lock); + + type = len > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + if (sensor->type == ST_LPS22HH_PRESS) + event = IIO_UNMOD_EVENT_CODE(IIO_PRESSURE, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + else + event = IIO_UNMOD_EVENT_CODE(IIO_TEMP, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(indio_dev, event, st_lps22hh_get_time_ns(hw)); + mutex_unlock(&indio_dev->mlock); + + return size; +} + + +#define ST_LPS22HH_EWMA_LEVEL 96 +#define ST_LPS22HH_EWMA_DIV 128 +static inline s64 st_lps22hh_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LPS22HH_EWMA_DIV - weight) * diff, + ST_LPS22HH_EWMA_DIV); + + return old + incr; +} + +static irqreturn_t st_lps22hh_irq_handler(int irq, void *private) +{ + struct st_lps22hh_hw *hw = private; + s64 delta_ts, ts = st_lps22hh_get_time_ns(hw); + + delta_ts = div_s64((ts - hw->ts_irq), hw->watermark); + if (hw->odr >= 50) + hw->delta_ts = st_lps22hh_ewma(hw->delta_ts, delta_ts, + ST_LPS22HH_EWMA_LEVEL); + else + hw->delta_ts = delta_ts; + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lps22hh_irq_thread(int irq, void *private) +{ + struct st_lps22hh_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_lps22hh_read_fifo(hw, hw->delta_ts); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_lps22hh_buffer_preenable(struct iio_dev *indio_dev) +{ + struct st_lps22hh_sensor *sensor = iio_priv(indio_dev); + struct st_lps22hh_hw *hw = sensor->hw; + int err; + + err = st_lps22hh_set_fifo_mode(sensor->hw, ST_LPS22HH_STREAM); + if (err < 0) + return err; + + err = st_lps22hh_update_fifo_watermark(hw, hw->watermark); + if (err < 0) + return err; + + err = st_lps22hh_write_with_mask(sensor->hw, ST_LPS22HH_CTRL3_ADDR, + ST_LPS22HH_INT_FTH_MASK, true); + if (err < 0) + return err; + + err = st_lps22hh_set_enable(sensor, true); + if (err < 0) + return err; + + hw->delta_ts = div_s64(1000000000UL, hw->odr); + hw->ts = st_lps22hh_get_time_ns(hw); + hw->ts_irq = hw->ts; + + return 0; +} + +static int st_lps22hh_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct st_lps22hh_sensor *sensor = iio_priv(indio_dev); + int err; + + err = st_lps22hh_set_fifo_mode(sensor->hw, ST_LPS22HH_BYPASS); + if (err < 0) + return err; + + err = st_lps22hh_write_with_mask(sensor->hw, ST_LPS22HH_CTRL3_ADDR, + ST_LPS22HH_INT_FTH_MASK, false); + if (err < 0) + return err; + + return st_lps22hh_set_enable(sensor, false); +} + +static const struct iio_buffer_setup_ops st_lps22hh_buffer_ops = { + .preenable = st_lps22hh_buffer_preenable, + .postdisable = st_lps22hh_buffer_postdisable, +}; + +int st_lps22hh_allocate_buffers(struct st_lps22hh_hw *hw) +{ +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + unsigned long irq_type; + u8 int_active = 0; + int err, i; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); + + switch (irq_type) { + case IRQF_TRIGGER_HIGH: + case IRQF_TRIGGER_RISING: + int_active = 0; + break; + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_FALLING: + int_active = 1; + break; + default: + dev_info(hw->dev, "mode %lx unsupported\n", irq_type); + return -EINVAL; + } + + err = st_lps22hh_write_with_mask(hw, ST_LPS22HH_CTRL2_ADDR, + ST_LPS22HH_INT_ACTIVE_MASK, + int_active); + if (err < 0) + return err; + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lps22hh_irq_handler, + st_lps22hh_irq_thread, + irq_type | IRQF_ONESHOT, + "lps22hh", hw); + if (err) + return err; + + for (i = 0; i < ST_LPS22HH_SENSORS_NUMB; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + INDIO_BUFFER_SOFTWARE, + &st_lps22hh_buffer_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_lps22hh_buffer_ops; +#endif /* LINUX_VERSION_CODE */ + + } + + return 0; +} diff --git a/drivers/iio/stm/pressure/st_lps22hh_core.c b/drivers/iio/stm/pressure/st_lps22hh_core.c new file mode 100644 index 000000000000..a9b01d99a7a3 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hh_core.c @@ -0,0 +1,492 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hh driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lps22hh.h" + +#define ST_LPS22HH_PRESS_FS_AVL_GAIN (1000000000UL / 4096UL) +#define ST_LPS22HH_TEMP_FS_AVL_GAIN 100 + +#define ST_LPS22HH_ODR_LIST_NUM 8 +struct st_lps22hh_odr_table_t { + u8 addr; + u8 mask; + u8 odr_avl[ST_LPS22HH_ODR_LIST_NUM]; +}; + +const static struct st_lps22hh_odr_table_t st_lps22hh_odr_table = { + .addr = 0x10, + .mask = 0x70, + .odr_avl = { 0, 1, 10, 25, 50, 75, 100, 200 }, +}; + +const struct iio_event_spec st_lps22hh_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lps22hh_press_channels[] = { + { + .type = IIO_PRESSURE, + .address = 0x28, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 24, + .storagebits = 32, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_PRESSURE, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps22hh_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lps22hh_temp_channels[] = { + { + .type = IIO_TEMP, + .address = 0x2b, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_TEMP, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps22hh_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +int st_lps22hh_write_with_mask(struct st_lps22hh_hw *hw, u8 addr, u8 mask, + u8 val) +{ + int err; + u8 data; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) + goto unlock; + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); +unlock: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_lps22hh_check_whoami(struct st_lps22hh_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_LPS22HH_WHO_AM_I_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + if (data != ST_LPS22HH_WHO_AM_I_DEF) { + dev_err(hw->dev, "Who-Am-I value not valid.\n"); + return -ENODEV; + } + return 0; +} + +static int st_lps22hh_get_odr(struct st_lps22hh_sensor *sensor, u8 odr) +{ + int i; + + for (i = 0; i < ST_LPS22HH_ODR_LIST_NUM; i++) { + if (st_lps22hh_odr_table.odr_avl[i] == odr) + break; + } + return i == ST_LPS22HH_ODR_LIST_NUM ? -EINVAL : i; +} + +int st_lps22hh_set_enable(struct st_lps22hh_sensor *sensor, bool enable) +{ + struct st_lps22hh_hw *hw = sensor->hw; + u32 max_odr = enable ? sensor->odr : 0; + int i; + + for (i = 0; i < ST_LPS22HH_SENSORS_NUMB; i++) { + if (sensor->type == i) + continue; + + if (hw->enable_mask & BIT(i)) { + struct st_lps22hh_sensor *temp; + + temp = iio_priv(hw->iio_devs[i]); + max_odr = max_t(u32, max_odr, temp->odr); + } + } + + if (max_odr != hw->odr) { + int err, ret; + + ret = st_lps22hh_get_odr(sensor, max_odr); + if (ret < 0) + return ret; + + err = st_lps22hh_write_with_mask(hw, st_lps22hh_odr_table.addr, + st_lps22hh_odr_table.mask, ret); + if (err < 0) + return err; + + hw->odr = max_odr; + } + + if (enable) + hw->enable_mask |= BIT(sensor->type); + else + hw->enable_mask &= ~BIT(sensor->type); + + return 0; +} + +int st_lps22hh_init_sensors(struct st_lps22hh_hw *hw) +{ + int err; + + /* Reboot memory content */ + err = st_lps22hh_write_with_mask(hw, ST_LPS22HH_CTRL2_ADDR, + ST_LPS22HH_BOOT_MASK, 1); + if (err < 0) + return err; + + msleep(10); + + /* soft reset the device on power on. */ + err = st_lps22hh_write_with_mask(hw, ST_LPS22HH_CTRL2_ADDR, + ST_LPS22HH_SOFT_RESET_MASK, 1); + if (err < 0) + return err; + + msleep(200); + + /* enable low noise */ + err = st_lps22hh_write_with_mask(hw, ST_LPS22HH_CTRL2_ADDR, + ST_LPS22HH_LOW_NOISE_EN_MASK, 1); + if (err < 0) + return err; + + /* enable latched interrupt mode */ + err = st_lps22hh_write_with_mask(hw, ST_LPS22HH_LIR_ADDR, + ST_LPS22HH_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable BDU */ + return st_lps22hh_write_with_mask(hw, ST_LPS22HH_CTRL1_ADDR, + ST_LPS22HH_BDU_MASK, 1); +} + +static ssize_t +st_lps22hh_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 1; i < ST_LPS22HH_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lps22hh_odr_table.odr_avl[i]); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lps22hh_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lps22hh_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->hw->watermark); +} + +static ssize_t +st_lps22hh_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +static ssize_t +st_lps22hh_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LPS22HH_MAX_FIFO_LENGTH); +} + +static int st_lps22hh_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lps22hh_sensor *sensor = iio_priv(indio_dev); + struct st_lps22hh_hw *hw = sensor->hw; + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + u8 len = ch->scan_type.realbits / 8; + u8 data[4] = {}; + + mutex_lock(&indio_dev->mlock); + if (iio_buffer_enabled(indio_dev)) { + mutex_unlock(&indio_dev->mlock); + ret = -EBUSY; + break; + } + + ret = st_lps22hh_set_enable(sensor, true); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + ret = -EBUSY; + break; + } + + msleep(40); + ret = hw->tf->read(hw->dev, ch->address, len, data); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + return ret; + } + + if (sensor->type == ST_LPS22HH_PRESS) + *val = (s32)get_unaligned_le32(data); + else if (sensor->type == ST_LPS22HH_TEMP) + *val = (s16)get_unaligned_le16(data); + + ret = st_lps22hh_set_enable(sensor, false); + mutex_unlock(&indio_dev->mlock); + + if (ret < 0) + return ret; + + ret = IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + switch (ch->type) { + case IIO_TEMP: + *val = 1000; + *val2 = sensor->gain; + ret = IIO_VAL_FRACTIONAL; + break; + case IIO_PRESSURE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + ret = -ENODEV; + break; + } + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static int st_lps22hh_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int val, int val2, long mask) +{ + struct st_lps22hh_sensor *sensor = iio_priv(indio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + ret = st_lps22hh_get_odr(sensor, val); + if (ret > 0) + sensor->odr = val; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lps22hh_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, + st_lps22hh_sysfs_get_hwfifo_watermark, + st_lps22hh_sysfs_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, + st_lps22hh_sysfs_get_hwfifo_watermark_min, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, + st_lps22hh_sysfs_get_hwfifo_watermark_max, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, + st_lps22hh_sysfs_flush_fifo, 0); + +static struct attribute *st_lps22hh_press_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static struct attribute *st_lps22hh_temp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lps22hh_press_attribute_group = { + .attrs = st_lps22hh_press_attributes, +}; +static const struct attribute_group st_lps22hh_temp_attribute_group = { + .attrs = st_lps22hh_temp_attributes, +}; + +static const struct iio_info st_lps22hh_press_info = { + .attrs = &st_lps22hh_press_attribute_group, + .read_raw = st_lps22hh_read_raw, + .write_raw = st_lps22hh_write_raw, +}; + +static const struct iio_info st_lps22hh_temp_info = { + .attrs = &st_lps22hh_temp_attribute_group, + .read_raw = st_lps22hh_read_raw, + .write_raw = st_lps22hh_write_raw, +}; + +int st_lps22hh_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps22hh_transfer_function *tf_ops) +{ + struct st_lps22hh_sensor *sensor; + struct st_lps22hh_hw *hw; + struct iio_dev *iio_dev; + int err, i; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + hw->dev = dev; + hw->tf = tf_ops; + hw->irq = irq; + /* set initial watermark */ + hw->watermark = 1; + + mutex_init(&hw->lock); + mutex_init(&hw->fifo_lock); + + err = st_lps22hh_check_whoami(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LPS22HH_SENSORS_NUMB; i++) { + iio_dev = devm_iio_device_alloc(dev, sizeof(*sensor)); + if (!iio_dev) + return -ENOMEM; + + hw->iio_devs[i] = iio_dev; + sensor = iio_priv(iio_dev); + sensor->hw = hw; + sensor->type = i; + sensor->odr = 1; + + switch (i) { + case ST_LPS22HH_PRESS: + sensor->gain = ST_LPS22HH_PRESS_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", name); + iio_dev->channels = st_lps22hh_press_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps22hh_press_channels); + iio_dev->info = &st_lps22hh_press_info; + break; + case ST_LPS22HH_TEMP: + sensor->gain = ST_LPS22HH_TEMP_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", name); + iio_dev->channels = st_lps22hh_temp_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps22hh_temp_channels); + iio_dev->info = &st_lps22hh_temp_info; + break; + default: + return -EINVAL; + }; + iio_dev->name = sensor->name; + iio_dev->modes = INDIO_DIRECT_MODE; + } + + err = st_lps22hh_init_sensors(hw); + if (err < 0) + return err; + + if (irq > 0) { + err = st_lps22hh_allocate_buffers(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LPS22HH_SENSORS_NUMB; i++) { + err = devm_iio_device_register(dev, hw->iio_devs[i]); + if (err) + return err; + } + return 0; +} +EXPORT_SYMBOL(st_lps22hh_common_probe); + +MODULE_DESCRIPTION("STMicroelectronics lps22hh driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22hh_i2c.c b/drivers/iio/stm/pressure/st_lps22hh_i2c.c new file mode 100644 index 000000000000..f9e806b37985 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hh_i2c.c @@ -0,0 +1,94 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hh i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include + +#include "st_lps22hh.h" + +static int st_lps22hh_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_lps22hh_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_lps22hh_transfer_function st_lps22hh_tf_i2c = { + .write = st_lps22hh_i2c_write, + .read = st_lps22hh_i2c_read, +}; + +static int st_lps22hh_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_lps22hh_common_probe(&client->dev, client->irq, client->name, + &st_lps22hh_tf_i2c); +} + +static const struct i2c_device_id st_lps22hh_ids[] = { + { "lps22ch" }, + { "lps22hh" }, + { "lps27hhw" }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_lps22hh_ids); + +static const struct of_device_id st_lps22hh_id_table[] = { + { .compatible = "st,lps22ch" }, + { .compatible = "st,lps22hh" }, + { .compatible = "st,lps27hhw" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps22hh_id_table); + +static struct i2c_driver st_lps22hh_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps22hh_i2c", + .of_match_table = of_match_ptr(st_lps22hh_id_table), + }, + .probe = st_lps22hh_i2c_probe, + .id_table = st_lps22hh_ids, +}; +module_i2c_driver(st_lps22hh_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics lps22hh i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps22hh_spi.c b/drivers/iio/stm/pressure/st_lps22hh_spi.c new file mode 100644 index 000000000000..ea9c3bbac31e --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps22hh_spi.c @@ -0,0 +1,101 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps22hh spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include + +#include "st_lps22hh.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lps22hh_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_lps22hh_hw *hw = spi_get_drvdata(spi); + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + int err; + + hw->tb.tx_buf[0] = addr | ST_SENSORS_SPI_READ; + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err) + return err; + + memcpy(data, hw->tb.rx_buf, len); + + return len; +} + +static int st_lps22hh_spi_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_lps22hh_hw *hw; + struct spi_device *spi; + + if (len >= ST_LPS22HH_TX_MAX_LENGTH) + return -ENOMEM; + + spi = to_spi_device(dev); + hw = spi_get_drvdata(spi); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_lps22hh_transfer_function st_lps22hh_tf_spi = { + .write = st_lps22hh_spi_write, + .read = st_lps22hh_spi_read, +}; + +static int st_lps22hh_spi_probe(struct spi_device *spi) +{ + return st_lps22hh_common_probe(&spi->dev, spi->irq, spi->modalias, + &st_lps22hh_tf_spi); +} + +static const struct spi_device_id st_lps22hh_ids[] = { + { "lps22ch" }, + { "lps22hh" }, + { "lps27hhw" }, + {} +}; +MODULE_DEVICE_TABLE(spi, st_lps22hh_ids); + +static const struct of_device_id st_lps22hh_id_table[] = { + { .compatible = "st,lps22ch" }, + { .compatible = "st,lps22hh" }, + { .compatible = "st,lps27hhw" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps22hh_id_table); + +static struct spi_driver st_lps22hh_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps22hh_spi", + .of_match_table = of_match_ptr(st_lps22hh_id_table), + }, + .probe = st_lps22hh_spi_probe, + .id_table = st_lps22hh_ids, +}; +module_spi_driver(st_lps22hh_spi_driver); + +MODULE_DESCRIPTION("STMicroelectronics lps22hh spi driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps33hw.h b/drivers/iio/stm/pressure/st_lps33hw.h new file mode 100644 index 000000000000..bbd226bdcad7 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps33hw.h @@ -0,0 +1,88 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics lps33hw driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#ifndef __ST_LPS33HW_H +#define __ST_LPS33HW_H + +#include +#include +#include +#include + +#define ST_LPS33HW_MAX_FIFO_LENGTH 31 + +#define ST_LPS33HW_CTRL3_ADDR 0x12 + +enum st_lps33hw_sensor_type { + ST_LPS33HW_PRESS = 0, + ST_LPS33HW_TEMP, + ST_LPS33HW_SENSORS_NUMB, +}; + +enum st_lps33hw_fifo_mode { + ST_LPS33HW_BYPASS = 0x0, + ST_LPS33HW_STREAM = 0x6, +}; + +#define ST_LPS33HW_TX_MAX_LENGTH 8 +#define ST_LPS33HW_RX_MAX_LENGTH 192 + +struct st_lps33hw_transfer_buffer { + u8 rx_buf[ST_LPS33HW_RX_MAX_LENGTH]; + u8 tx_buf[ST_LPS33HW_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_lps33hw_transfer_function { + int (*write)(struct device *dev, u8 addr, int len, u8 *data); + int (*read)(struct device *dev, u8 addr, int len, u8 *data); +}; + +struct st_lps33hw_hw { + struct device *dev; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + u8 watermark; + + struct iio_dev *iio_devs[ST_LPS33HW_SENSORS_NUMB]; + u8 enable_mask; + u8 odr; + + s64 delta_ts; + s64 ts_irq; + s64 ts; + + const struct st_lps33hw_transfer_function *tf; + struct st_lps33hw_transfer_buffer tb; +}; + +struct st_lps33hw_sensor { + struct st_lps33hw_hw *hw; + enum st_lps33hw_sensor_type type; + char name[32]; + + u32 gain; + u8 odr; +}; + +int st_lps33hw_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps33hw_transfer_function *tf_ops); +int st_lps33hw_write_with_mask(struct st_lps33hw_hw *hw, u8 addr, u8 mask, + u8 data); +int st_lps33hw_allocate_buffers(struct st_lps33hw_hw *hw); +int st_lps33hw_set_enable(struct st_lps33hw_sensor *sensor, bool enable); +ssize_t st_lps33hw_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count); +ssize_t st_lps33hw_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size); + +#endif /* __ST_LPS33HW_H */ diff --git a/drivers/iio/stm/pressure/st_lps33hw_buffer.c b/drivers/iio/stm/pressure/st_lps33hw_buffer.c new file mode 100644 index 000000000000..b8a947c10d48 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps33hw_buffer.c @@ -0,0 +1,286 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps33hw buffer driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_lps33hw.h" + +#define ST_LPS33HW_FIFO_CTRL_ADDR 0x14 +#define ST_LPS33HW_FIFO_THS_MASK 0x1f +#define ST_LPS33HW_FIFO_MODE_MASK 0xe0 +#define ST_LPS33HW_INT_FTH_MASK 0x10 + +#define ST_LPS33HW_FIFO_SRC_ADDR 0x26 +#define ST_LPS33HW_FIFO_SRC_DIFF_MASK 0x1f + +#define ST_LPS33HW_PRESS_OUT_XL_ADDR 0x28 + +#define ST_LPS33HW_PRESS_SAMPLE_LEN 3 +#define ST_LPS33HW_TEMP_SAMPLE_LEN 2 +#define ST_LPS33HW_FIFO_SAMPLE_LEN (ST_LPS33HW_PRESS_SAMPLE_LEN + \ + ST_LPS33HW_TEMP_SAMPLE_LEN) + +static inline s64 st_lps33hw_get_time_ns(struct iio_dev *iio_dev) +{ + return iio_get_time_ns(iio_dev); +} + +#define ST_LPS33HW_EWMA_LEVEL 96 +#define ST_LPS33HW_EWMA_DIV 128 +static inline s64 st_lps33hw_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LPS33HW_EWMA_DIV - weight) * diff, + ST_LPS33HW_EWMA_DIV); + + return old + incr; +} + +static int st_lps33hw_set_fifo_mode(struct st_lps33hw_hw *hw, + enum st_lps33hw_fifo_mode mode) +{ + switch (mode) { + case ST_LPS33HW_BYPASS: + case ST_LPS33HW_STREAM: + break; + default: + return -EINVAL; + } + return st_lps33hw_write_with_mask(hw, ST_LPS33HW_FIFO_CTRL_ADDR, + ST_LPS33HW_FIFO_MODE_MASK, mode); +} + +static int st_lps33hw_update_fifo_watermark(struct st_lps33hw_hw *hw, u8 val) +{ + int err; + + err = st_lps33hw_write_with_mask(hw, ST_LPS33HW_FIFO_CTRL_ADDR, + ST_LPS33HW_FIFO_THS_MASK, val); + if (err < 0) + return err; + + hw->watermark = val; + + return 0; +} + +ssize_t st_lps33hw_sysfs_set_hwfifo_watermark(struct device * dev, + struct device_attribute * attr, + const char *buf, size_t count) +{ + struct st_lps33hw_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + int err, watermark; + + err = kstrtoint(buf, 10, &watermark); + if (err < 0) + return err; + + if (watermark < 1 || watermark > ST_LPS33HW_MAX_FIFO_LENGTH) + return -EINVAL; + + err = st_lps33hw_update_fifo_watermark(sensor->hw, watermark); + + return err < 0 ? err : count; +} + +static int st_lps33hw_read_fifo(struct st_lps33hw_hw *hw) +{ + u8 iio_buff[ALIGN(sizeof(u32) + sizeof(s64), sizeof(s64))]; + u8 status, buff[ST_LPS33HW_RX_MAX_LENGTH]; + int err, i, read_len; + + err = hw->tf->read(hw->dev, ST_LPS33HW_FIFO_SRC_ADDR, + sizeof(status), &status); + if (err < 0) + return err; + + read_len = (status & ST_LPS33HW_FIFO_SRC_DIFF_MASK) * + ST_LPS33HW_FIFO_SAMPLE_LEN; + if (!read_len) + return 0; + + err = hw->tf->read(hw->dev, ST_LPS33HW_PRESS_OUT_XL_ADDR, + read_len, buff); + if (err < 0) + return err; + + for (i = 0; i < read_len; i += ST_LPS33HW_FIFO_SAMPLE_LEN) { + memcpy(iio_buff, buff + i, ST_LPS33HW_PRESS_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS33HW_PRESS], + iio_buff, hw->ts); + /* temp sample */ + memcpy(iio_buff, buff + i + ST_LPS33HW_PRESS_SAMPLE_LEN, + ST_LPS33HW_TEMP_SAMPLE_LEN); + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LPS33HW_TEMP], + iio_buff, hw->ts); + hw->ts += hw->delta_ts; + } + + return read_len; +} + +ssize_t st_lps33hw_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct st_lps33hw_sensor *sensor = iio_priv(indio_dev); + struct st_lps33hw_hw *hw = sensor->hw; + u64 type, event; + int len; + + mutex_lock(&indio_dev->mlock); + if (!iio_buffer_enabled(indio_dev)) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + mutex_lock(&hw->fifo_lock); + len = st_lps33hw_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + type = len > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY; + if (sensor->type == ST_LPS33HW_PRESS) + event = IIO_UNMOD_EVENT_CODE(IIO_PRESSURE, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + else + event = IIO_UNMOD_EVENT_CODE(IIO_TEMP, -1, + IIO_EV_TYPE_FIFO_FLUSH, type); + iio_push_event(indio_dev, event, st_lps33hw_get_time_ns(indio_dev)); + mutex_unlock(&indio_dev->mlock); + + return size; +} + + +static irqreturn_t st_lps33hw_irq_handler(int irq, void *private) +{ + struct st_lps33hw_hw *hw = private; + s64 delta_ts, ts; + + ts = st_lps33hw_get_time_ns(hw->iio_devs[ST_LPS33HW_PRESS]); + delta_ts = div_s64((ts - hw->ts_irq), hw->watermark); + if (hw->odr >= 50) + hw->delta_ts = st_lps33hw_ewma(hw->delta_ts, delta_ts, + ST_LPS33HW_EWMA_LEVEL); + else + hw->delta_ts = delta_ts; + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lps33hw_irq_thread(int irq, void *private) +{ + struct st_lps33hw_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_lps33hw_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_lps33hw_buffer_preenable(struct iio_dev *indio_dev) +{ + struct st_lps33hw_sensor *sensor = iio_priv(indio_dev); + struct st_lps33hw_hw *hw = sensor->hw; + int err; + + err = st_lps33hw_set_fifo_mode(sensor->hw, ST_LPS33HW_STREAM); + if (err < 0) + return err; + + err = st_lps33hw_update_fifo_watermark(hw, hw->watermark); + if (err < 0) + return err; + + err = st_lps33hw_write_with_mask(sensor->hw, ST_LPS33HW_CTRL3_ADDR, + ST_LPS33HW_INT_FTH_MASK, true); + if (err < 0) + return err; + + err = st_lps33hw_set_enable(sensor, true); + if (err < 0) + return err; + + hw->delta_ts = div_s64(1000000000UL, hw->odr); + hw->ts = st_lps33hw_get_time_ns(indio_dev); + hw->ts_irq = hw->ts; + + return 0; +} + +static int st_lps33hw_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct st_lps33hw_sensor *sensor = iio_priv(indio_dev); + int err; + + err = st_lps33hw_set_fifo_mode(sensor->hw, ST_LPS33HW_BYPASS); + if (err < 0) + return err; + + err = st_lps33hw_write_with_mask(sensor->hw, ST_LPS33HW_CTRL3_ADDR, + ST_LPS33HW_INT_FTH_MASK, false); + if (err < 0) + return err; + + return st_lps33hw_set_enable(sensor, false); +} + +static const struct iio_buffer_setup_ops st_lps33hw_buffer_ops = { + .preenable = st_lps33hw_buffer_preenable, + .postdisable = st_lps33hw_buffer_postdisable, +}; + +int st_lps33hw_allocate_buffers(struct st_lps33hw_hw *hw) +{ +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + int err, i; + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lps33hw_irq_handler, + st_lps33hw_irq_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + "lps33hw", hw); + if (err) + return err; + + for (i = 0; i < ST_LPS33HW_SENSORS_NUMB; i++) { + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + INDIO_BUFFER_SOFTWARE, + &st_lps33hw_buffer_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_lps33hw_buffer_ops; +#endif /* LINUX_VERSION_CODE */ + + } + return 0; +} diff --git a/drivers/iio/stm/pressure/st_lps33hw_core.c b/drivers/iio/stm/pressure/st_lps33hw_core.c new file mode 100644 index 000000000000..55a9ee6893e1 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps33hw_core.c @@ -0,0 +1,496 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps33hw driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lps33hw.h" + +#define ST_LPS33HW_WHO_AM_I_ADDR 0x0f +#define ST_LPS33HW_WHO_AM_I_DEF 0xb1 + +#define ST_LPS33HW_CTRL1_ADDR 0x10 +#define ST_LPS33HW_BDU_MASK 0x02 +#define ST_LPS33HW_CTRL2_ADDR 0x11 +#define ST_LPS33HW_SOFT_RESET_MASK 0x04 +#define ST_LPS33HW_FIFO_ENABLE_MASK 0x40 + +#define ST_LPS33HW_LIR_ADDR 0x0b +#define ST_LPS33HW_LIR_MASK 0x04 + +#define ST_LPS33HW_PRESS_FS_AVL_GAIN (1000000000UL / 4096UL) +#define ST_LPS33HW_TEMP_FS_AVL_GAIN 100 + +#define ST_LPS33HW_ODR_LIST_NUM 6 +struct st_lps33hw_odr_table_t { + u8 addr; + u8 mask; + u8 odr_avl[ST_LPS33HW_ODR_LIST_NUM]; +}; + +const static struct st_lps33hw_odr_table_t st_lps33hw_odr_table = { + .addr = 0x10, + .mask = 0x70, + .odr_avl = { 0, 1, 10, 25, 50, 75 }, +}; + +const struct iio_event_spec st_lps33hw_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lps33hw_press_channels[] = { + { + .type = IIO_PRESSURE, + .address = 0x28, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 24, + .storagebits = 32, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_PRESSURE, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps33hw_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lps33hw_temp_channels[] = { + { + .type = IIO_TEMP, + .address = 0x2b, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .channel2 = IIO_NO_MOD, + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_TEMP, + .scan_index = -1, + .indexed = -1, + .event_spec = &st_lps33hw_fifo_flush_event, + .num_event_specs = 1, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +int st_lps33hw_write_with_mask(struct st_lps33hw_hw *hw, u8 addr, u8 mask, + u8 val) +{ + int err; + u8 data; + + mutex_lock(&hw->lock); + + err = hw->tf->read(hw->dev, addr, sizeof(data), &data); + if (err < 0) + goto unlock; + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + err = hw->tf->write(hw->dev, addr, sizeof(data), &data); +unlock: + mutex_unlock(&hw->lock); + + return err; +} + +static int st_lps33hw_check_whoami(struct st_lps33hw_hw *hw) +{ + int err; + u8 data; + + err = hw->tf->read(hw->dev, ST_LPS33HW_WHO_AM_I_ADDR, sizeof(data), + &data); + if (err < 0) { + dev_err(hw->dev, "failed to read Who-Am-I register.\n"); + + return err; + } + if (data != ST_LPS33HW_WHO_AM_I_DEF) { + dev_err(hw->dev, "Who-Am-I value not valid.\n"); + return -ENODEV; + } + return 0; +} + +static int st_lps33hw_get_odr(struct st_lps33hw_sensor *sensor, u8 odr) +{ + int i; + + for (i = 0; i < ST_LPS33HW_ODR_LIST_NUM; i++) { + if (st_lps33hw_odr_table.odr_avl[i] == odr) + break; + } + return i == ST_LPS33HW_ODR_LIST_NUM ? -EINVAL : i; +} + +int st_lps33hw_set_enable(struct st_lps33hw_sensor *sensor, bool enable) +{ + struct st_lps33hw_hw *hw = sensor->hw; + u32 max_odr = enable ? sensor->odr : 0; + int i; + + for (i = 0; i < ST_LPS33HW_SENSORS_NUMB; i++) { + if (sensor->type == i) + continue; + + if (hw->enable_mask & BIT(i)) { + struct st_lps33hw_sensor *temp; + + temp = iio_priv(hw->iio_devs[i]); + max_odr = max_t(u32, max_odr, temp->odr); + } + } + + if (max_odr != hw->odr) { + int err, ret; + + ret = st_lps33hw_get_odr(sensor, max_odr); + if (ret < 0) + return ret; + + err = st_lps33hw_write_with_mask(hw, st_lps33hw_odr_table.addr, + st_lps33hw_odr_table.mask, ret); + if (err < 0) + return err; + + hw->odr = max_odr; + } + + if (enable) + hw->enable_mask |= BIT(sensor->type); + else + hw->enable_mask &= ~BIT(sensor->type); + + return 0; +} + +int st_lps33hw_init_sensors(struct st_lps33hw_hw *hw) +{ + int err; + + /* soft reset the device on power on. */ + err = st_lps33hw_write_with_mask(hw, ST_LPS33HW_CTRL2_ADDR, + ST_LPS33HW_SOFT_RESET_MASK, 1); + if (err < 0) + return err; + + msleep(200); + + /* enable latched interrupt mode */ + err = st_lps33hw_write_with_mask(hw, ST_LPS33HW_LIR_ADDR, + ST_LPS33HW_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable FIFO */ + err = st_lps33hw_write_with_mask(hw, ST_LPS33HW_CTRL2_ADDR, + ST_LPS33HW_FIFO_ENABLE_MASK, 1); + if (err < 0) + return err; + + /* enable BDU */ + return st_lps33hw_write_with_mask(hw, ST_LPS33HW_CTRL1_ADDR, + ST_LPS33HW_BDU_MASK, 1); +} + +static ssize_t +st_lps33hw_get_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 1; i < ST_LPS33HW_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lps33hw_odr_table.odr_avl[i]); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t +st_lps33hw_sysfs_get_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lps33hw_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sensor->hw->watermark); +} + +static ssize_t +st_lps33hw_sysfs_get_hwfifo_watermark_min(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", 1); +} + +static ssize_t +st_lps33hw_sysfs_get_hwfifo_watermark_max(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LPS33HW_MAX_FIFO_LENGTH); +} + +static int st_lps33hw_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lps33hw_sensor *sensor = iio_priv(indio_dev); + struct st_lps33hw_hw *hw = sensor->hw; + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + u8 len = ch->scan_type.realbits / 8; + u8 data[4] = {}; + + mutex_lock(&indio_dev->mlock); + if (iio_buffer_enabled(indio_dev)) { + mutex_unlock(&indio_dev->mlock); + ret = -EBUSY; + break; + } + + ret = st_lps33hw_set_enable(sensor, true); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + ret = -EBUSY; + break; + } + + msleep(40); + ret = hw->tf->read(hw->dev, ch->address, len, data); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + return ret; + } + + if (sensor->type == ST_LPS33HW_PRESS) + *val = (s32)get_unaligned_le32(data); + else if (sensor->type == ST_LPS33HW_TEMP) + *val = (s16)get_unaligned_le16(data); + + ret = st_lps33hw_set_enable(sensor, false); + mutex_unlock(&indio_dev->mlock); + + if (ret < 0) + return ret; + + ret = IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + switch (ch->type) { + case IIO_TEMP: + *val = 1000; + *val2 = sensor->gain; + ret = IIO_VAL_FRACTIONAL; + break; + case IIO_PRESSURE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + ret = -ENODEV; + break; + } + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static int st_lps33hw_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, + int val, int val2, long mask) +{ + struct st_lps33hw_sensor *sensor = iio_priv(indio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + ret = st_lps33hw_get_odr(sensor, val); + if (ret > 0) + sensor->odr = val; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lps33hw_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, + st_lps33hw_sysfs_get_hwfifo_watermark, + st_lps33hw_sysfs_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, + st_lps33hw_sysfs_get_hwfifo_watermark_min, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, + st_lps33hw_sysfs_get_hwfifo_watermark_max, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, + st_lps33hw_sysfs_flush_fifo, 0); + +static struct attribute *st_lps33hw_press_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static struct attribute *st_lps33hw_temp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_min.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lps33hw_press_attribute_group = { + .attrs = st_lps33hw_press_attributes, +}; +static const struct attribute_group st_lps33hw_temp_attribute_group = { + .attrs = st_lps33hw_temp_attributes, +}; + +static const struct iio_info st_lps33hw_press_info = { + .attrs = &st_lps33hw_press_attribute_group, + .read_raw = st_lps33hw_read_raw, + .write_raw = st_lps33hw_write_raw, +}; + +static const struct iio_info st_lps33hw_temp_info = { + .attrs = &st_lps33hw_temp_attribute_group, + .read_raw = st_lps33hw_read_raw, + .write_raw = st_lps33hw_write_raw, +}; + +int st_lps33hw_common_probe(struct device *dev, int irq, const char *name, + const struct st_lps33hw_transfer_function *tf_ops) +{ + struct st_lps33hw_sensor *sensor; + struct st_lps33hw_hw *hw; + struct iio_dev *iio_dev; + int err, i; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + hw->dev = dev; + hw->tf = tf_ops; + hw->irq = irq; + /* set initial watermark */ + hw->watermark = 1; + + mutex_init(&hw->lock); + mutex_init(&hw->fifo_lock); + + err = st_lps33hw_check_whoami(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_LPS33HW_SENSORS_NUMB; i++) { + iio_dev = devm_iio_device_alloc(dev, sizeof(*sensor)); + if (!iio_dev) + return -ENOMEM; + + hw->iio_devs[i] = iio_dev; + sensor = iio_priv(iio_dev); + sensor->hw = hw; + sensor->type = i; + sensor->odr = 1; + + switch (i) { + case ST_LPS33HW_PRESS: + sensor->gain = ST_LPS33HW_PRESS_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_press", name); + iio_dev->channels = st_lps33hw_press_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps33hw_press_channels); + iio_dev->info = &st_lps33hw_press_info; + break; + case ST_LPS33HW_TEMP: + sensor->gain = ST_LPS33HW_TEMP_FS_AVL_GAIN; + scnprintf(sensor->name, sizeof(sensor->name), + "%s_temp", name); + iio_dev->channels = st_lps33hw_temp_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_lps33hw_temp_channels); + iio_dev->info = &st_lps33hw_temp_info; + break; + default: + return -EINVAL; + }; + iio_dev->name = sensor->name; + iio_dev->modes = INDIO_DIRECT_MODE; + } + + err = st_lps33hw_init_sensors(hw); + if (err < 0) + return err; + + if (irq > 0) { + err = st_lps33hw_allocate_buffers(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LPS33HW_SENSORS_NUMB; i++) { + err = devm_iio_device_register(dev, hw->iio_devs[i]); + if (err) + return err; + } + return 0; +} +EXPORT_SYMBOL(st_lps33hw_common_probe); + +MODULE_DESCRIPTION("STMicroelectronics lps33hw driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps33hw_i2c.c b/drivers/iio/stm/pressure/st_lps33hw_i2c.c new file mode 100644 index 000000000000..ed08ad2d4deb --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps33hw_i2c.c @@ -0,0 +1,90 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps33hw i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include + +#include "st_lps33hw.h" + +static int st_lps33hw_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_lps33hw_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[4]; + + if (len >= ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_lps33hw_transfer_function st_lps33hw_tf_i2c = { + .write = st_lps33hw_i2c_write, + .read = st_lps33hw_i2c_read, +}; + +static int st_lps33hw_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + return st_lps33hw_common_probe(&client->dev, client->irq, client->name, + &st_lps33hw_tf_i2c); +} + +static const struct i2c_device_id st_lps33hw_ids[] = { + { "lps33hw" }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_lps33hw_ids); + +static const struct of_device_id st_lps33hw_id_table[] = { + { .compatible = "st,lps33hw" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps33hw_id_table); + +static struct i2c_driver st_lps33hw_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps33hw_i2c", + .of_match_table = of_match_ptr(st_lps33hw_id_table), + }, + .probe = st_lps33hw_i2c_probe, + .id_table = st_lps33hw_ids, +}; +module_i2c_driver(st_lps33hw_i2c_driver); + +MODULE_DESCRIPTION("STMicroelectronics lps33hw i2c driver"); +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/pressure/st_lps33hw_spi.c b/drivers/iio/stm/pressure/st_lps33hw_spi.c new file mode 100644 index 000000000000..0ce1162b6f95 --- /dev/null +++ b/drivers/iio/stm/pressure/st_lps33hw_spi.c @@ -0,0 +1,97 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics lps33hw spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2017 STMicroelectronics Inc. + */ + +#include + +#include "st_lps33hw.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lps33hw_spi_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(dev); + struct st_lps33hw_hw *hw = spi_get_drvdata(spi); + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + int err; + + hw->tb.tx_buf[0] = addr | ST_SENSORS_SPI_READ; + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err) + return err; + + memcpy(data, hw->tb.rx_buf, len); + + return len; +} + +static int st_lps33hw_spi_write(struct device *dev, u8 addr, int len, u8 *data) +{ + struct st_lps33hw_hw *hw; + struct spi_device *spi; + + if (len >= ST_LPS33HW_TX_MAX_LENGTH) + return -ENOMEM; + + spi = to_spi_device(dev); + hw = spi_get_drvdata(spi); + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +static const struct st_lps33hw_transfer_function st_lps33hw_tf_spi = { + .write = st_lps33hw_spi_write, + .read = st_lps33hw_spi_read, +}; + +static int st_lps33hw_spi_probe(struct spi_device *spi) +{ + return st_lps33hw_common_probe(&spi->dev, spi->irq, spi->modalias, + &st_lps33hw_tf_spi); +} + +static const struct spi_device_id st_lps33hw_ids[] = { + { "lps33hw" }, + {} +}; +MODULE_DEVICE_TABLE(spi, st_lps33hw_ids); + +static const struct of_device_id st_lps33hw_id_table[] = { + { .compatible = "st,lps33hw" }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lps33hw_id_table); + +static struct spi_driver st_lps33hw_spi_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st_lps33hw_spi", + .of_match_table = of_match_ptr(st_lps33hw_id_table), + }, + .probe = st_lps33hw_spi_probe, + .id_table = st_lps33hw_ids, +}; +module_spi_driver(st_lps33hw_spi_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics lps33hw spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/temperature/Kconfig b/drivers/iio/stm/temperature/Kconfig new file mode 100644 index 000000000000..5912fb274fcd --- /dev/null +++ b/drivers/iio/stm/temperature/Kconfig @@ -0,0 +1,17 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Temperature sensor drivers +# +menu "Temperature sensors" + +config IIO_STTS22H + tristate "STTS22H ST MEMS temperature sensor" + depends on I2C + help + If you say yes here you get support for STTS22H ST MEMS I2C + temperature sensor. + + This driver can also be built as a module. If so, the module + will be called stts22h. + +endmenu diff --git a/drivers/iio/stm/temperature/Makefile b/drivers/iio/stm/temperature/Makefile new file mode 100644 index 000000000000..617b2a864d65 --- /dev/null +++ b/drivers/iio/stm/temperature/Makefile @@ -0,0 +1,6 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for industrial I/O temperature drivers +# + +obj-$(CONFIG_IIO_STTS22H) += stts22h.o diff --git a/drivers/iio/stm/temperature/stts22h.c b/drivers/iio/stm/temperature/stts22h.c new file mode 100644 index 000000000000..db27cc2b2974 --- /dev/null +++ b/drivers/iio/stm/temperature/stts22h.c @@ -0,0 +1,624 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * STMicroelectronics stts22h temperature driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "stts22h.h" + +static const struct iio_chan_spec st_stts22h_channel[] = { + { + .type = IIO_TEMP, + .address = ST_STTS22H_TEMP_L_OUT_ADDR, + .modified = 1, + .channel2 = IIO_MOD_TEMP_AMBIENT, + .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 = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; +static const unsigned long st_stts22h_available_scan_masks[] = { 0x1, 0x0 }; + +static const struct st_stts22h_odr_table_entry { + u8 size; + struct st_stts22h_reg reg; + struct st_stts22h_odr odr_avl[ST_STTS22H_ODR_LIST_SIZE]; +} st_stts22h_odr_table = { + .size = ST_STTS22H_ODR_LIST_SIZE, + .reg = { + .addr = ST_STTS22H_CTRL_ADDR, + .mask = ST_STTS22H_AVG_MASK, + }, + .odr_avl[0] = { 25, 0x00 }, + .odr_avl[1] = { 50, 0x01 }, + .odr_avl[2] = { 100, 0x02 }, + .odr_avl[3] = { 200, 0x03 }, +}; + +static int st_stts22h_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_stts22h_write(struct device *dev, u8 addr, int len, + const u8 *data) +{ + struct i2c_client *client = to_i2c_client(dev); + struct i2c_msg msg; + u8 send[4]; + + if (len > ARRAY_SIZE(send)) + return -ENOMEM; + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static inline int st_stts22h_write_with_mask(struct st_stts22h_data *data, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 read; + + mutex_lock(&data->lock); + err = st_stts22h_read(data->dev, addr, sizeof(read), &read); + if (err < 0) { + dev_err(data->dev, "failed to read %02x register\n", addr); + goto out; + } + + read = (read & ~mask) | ((val << __ffs(mask)) & mask); + + err = st_stts22h_write(data->dev, addr, sizeof(read), &read); + if (err < 0) + dev_err(data->dev, "failed to write %02x register\n", addr); + +out: + mutex_unlock(&data->lock); + + return err; +} + +static __maybe_unused int st_stts22h_reg_access(struct iio_dev *iio_dev, + unsigned int reg, + unsigned int writeval, + unsigned int *readval) +{ + struct st_stts22h_data *data = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + mutex_lock(&data->lock); + if (readval == NULL) + err = st_stts22h_write(data->dev, reg, 1, (u8 *)&writeval); + else + err = st_stts22h_read(data->dev, reg, 1, (u8 *)readval); + mutex_unlock(&data->lock); + + iio_device_release_direct_mode(iio_dev); + + return (err < 0) ? err : 0; +} + +static inline void st_stts22h_flush_works(struct st_stts22h_data *data) +{ + flush_workqueue(data->st_stts22h_workqueue); +} + +static int st_stts22h_allocate_workqueue(struct st_stts22h_data *data) +{ + if (!data->st_stts22h_workqueue) + data->st_stts22h_workqueue = + create_workqueue(data->iio_devs->name); + + if (!data->st_stts22h_workqueue) + return -ENOMEM; + + return 0; +} + +static inline s64 st_stts22h_get_time_ns(struct st_stts22h_data *data) +{ + return iio_get_time_ns(data->iio_devs); +} + +static ssize_t +st_stts22h_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < st_stts22h_odr_table.size; i++) { + if (!st_stts22h_odr_table.odr_avl[i].hz) + continue; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_stts22h_odr_table.odr_avl[i].hz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static int st_stts22h_get_odr_val(struct st_stts22h_data *data, int val, + u8 *odr) +{ + int i; + + for (i = 0; i < st_stts22h_odr_table.size; i++) { + if (st_stts22h_odr_table.odr_avl[i].hz >= val) + break; + } + + if (i == st_stts22h_odr_table.size) + return -EINVAL; + + *odr = st_stts22h_odr_table.odr_avl[i].hz; + + return i; +} + +static int st_stts22h_set_odr(struct st_stts22h_data *data, u8 req_odr) +{ + int err, i; + + for (i = 0; i < st_stts22h_odr_table.size; i++) { + if (st_stts22h_odr_table.odr_avl[i].hz >= req_odr) + break; + } + + if (i == st_stts22h_odr_table.size) + return -EINVAL; + + err = st_stts22h_write_with_mask(data, st_stts22h_odr_table.reg.addr, + st_stts22h_odr_table.reg.mask, + st_stts22h_odr_table.odr_avl[i].val); + + return err < 0 ? err : 0; +} + +static int st_stts22h_sensor_set_enable(struct st_stts22h_data *data, bool en) +{ + u8 odr = en ? data->odr : 0; + int64_t newTime; + int err; + + err = st_stts22h_set_odr(data, odr); + if (err < 0) + return err; + + err = st_stts22h_write_with_mask(data, + ST_STTS22H_CTRL_ADDR, + ST_STTS22H_FREERUN_MASK, + en); + if (err < 0) + return err; + + if (en) { + newTime = HZ_TO_PERIOD_NSEC(odr); + data->sensorktime = ktime_set(0, newTime); + hrtimer_start(&data->hr_timer, data->sensorktime, + HRTIMER_MODE_REL); + + } else { + cancel_work_sync(&data->iio_work); + hrtimer_cancel(&data->hr_timer); + } + + data->enable = en; + + return 0; +} + +static int st_stts22h_read_oneshot(struct st_stts22h_data *data, + u8 addr, int *val) +{ + int err, delay; + __le16 temp; + + err = st_stts22h_sensor_set_enable(data, true); + if (err < 0) + return err; + + delay = 2 * (1000000 / data->odr); + usleep_range(delay, 2 * delay); + + err = st_stts22h_read(data->dev, addr, sizeof(temp), (u8 *)&temp); + if (err < 0) + return err; + + st_stts22h_sensor_set_enable(data, false); + + *val = (s16)le16_to_cpu(temp); + + return IIO_VAL_INT; +} + +static int st_stts22h_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_stts22h_data *data = iio_priv(iio_dev); + int err; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + err = st_stts22h_read_oneshot(data, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = (int)data->odr; + err = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 1000; + *val2 = ST_STTS22H_GAIN; + err = IIO_VAL_FRACTIONAL; + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +static int st_stts22h_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + int err = -EINVAL; + + if (mask == IIO_CHAN_INFO_SAMP_FREQ) { + struct st_stts22h_data *data = iio_priv(iio_dev); + u8 odr; + + + err = st_stts22h_get_odr_val(data, val, &odr); + if (err < 0) + return err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + data->odr = odr; + iio_device_release_direct_mode(iio_dev); + } + + return err < 0 ? err : 0; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_stts22h_sysfs_sampling_frequency_avail); +static struct attribute *st_stts22h_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_stts22h_attribute_group = { + .attrs = st_stts22h_attributes, +}; + +static const struct iio_info st_stts22h_info = { + .attrs = &st_stts22h_attribute_group, + .read_raw = st_stts22h_read_raw, + .write_raw = st_stts22h_write_raw, + +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_stts22h_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static int st_stts22h_check_whoami(struct st_stts22h_data *data) +{ + int err; + u8 wai; + + err = st_stts22h_read(data->dev, ST_STTS22H_WHOAMI_ADDR, 1, &wai); + if (err < 0) + return err; + + if (wai != ST_STTS22H_WHOAMI_VAL) { + dev_err(data->dev, "unsupported whoami [%02x]\n", wai); + + return -ENODEV; + } + + return 0; +} + +static int st_stts22h_init(struct st_stts22h_data *data) +{ + int err; + + /* reset cycle */ + err = st_stts22h_write_with_mask(data, ST_STTS22H_CTRL_ADDR, + ST_STTS22H_SW_RESET_MASK, 1); + if (err < 0) + return err; + + err = st_stts22h_write_with_mask(data, ST_STTS22H_CTRL_ADDR, + ST_STTS22H_SW_RESET_MASK, 0); + if (err < 0) + return err; + + /* enable bdu */ + err = st_stts22h_write_with_mask(data, ST_STTS22H_CTRL_ADDR, + ST_STTS22H_BDU_MASK, 1); + if (err < 0) + return err; + + /* enable register auto increments */ + err = st_stts22h_write_with_mask(data, ST_STTS22H_CTRL_ADDR, + ST_STTS22H_IF_ADD_INC_MASK, 1); + + return err < 0 ? err : 0; +} + +static +enum hrtimer_restart st_stts22h_poll_function_read(struct hrtimer *timer) +{ + struct st_stts22h_data *data; + + data = container_of((struct hrtimer *)timer, + struct st_stts22h_data, hr_timer); + + data->timestamp = st_stts22h_get_time_ns(data); + queue_work(data->st_stts22h_workqueue, &data->iio_work); + + return HRTIMER_NORESTART; +} + +static void st_stts22h_report_event(struct st_stts22h_data *data, u8 *tmp) +{ + struct iio_dev *iio_dev = data->iio_devs; + u8 iio_buf[ALIGN(ST_STTS22H_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)]; + + memcpy(iio_buf, tmp, ST_STTS22H_SAMPLE_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, data->timestamp); +} + +static void st_stts22h_poll_function_work(struct work_struct *iio_work) +{ + struct st_stts22h_data *data; + ktime_t tmpkt, ktdelta; + __le16 temp; + + data = container_of((struct work_struct *)iio_work, + struct st_stts22h_data, iio_work); + + /* adjust new timeout */ + ktdelta = ktime_set(0, (st_stts22h_get_time_ns(data) - + data->timestamp)); + + /* avoid negative value in case of high ODRs */ + if (ktime_after(data->sensorktime, ktdelta)) + tmpkt = ktime_sub(data->sensorktime, ktdelta); + else + tmpkt = data->sensorktime; + + hrtimer_start(&data->hr_timer, tmpkt, HRTIMER_MODE_REL); + + st_stts22h_read(data->dev, ST_STTS22H_TEMP_L_OUT_ADDR, + sizeof(temp), (u8 *)&temp); + st_stts22h_report_event(data, (u8 *)&temp); +} + +static int st_stts22h_preenable(struct iio_dev *iio_dev) +{ + struct st_stts22h_data *data = iio_priv(iio_dev); + + return st_stts22h_sensor_set_enable(data, true); +} + +static int st_stts22h_postdisable(struct iio_dev *iio_dev) +{ + struct st_stts22h_data *data = iio_priv(iio_dev); + + return st_stts22h_sensor_set_enable(data, false); +} + +static const struct iio_buffer_setup_ops st_stts22h_buffer_ops = { + .preenable = st_stts22h_preenable, + .postdisable = st_stts22h_postdisable, +}; + +static int st_stts22h_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct st_stts22h_data *data; +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,13,0) + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + struct iio_dev *iio_dev; + struct device *dev; + int err; + + iio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data)); + if (!iio_dev) + return -ENOMEM; + + data = iio_priv(iio_dev); + i2c_set_clientdata(client, iio_dev); + + dev = &client->dev; + data->dev = dev; + dev_set_drvdata(dev, (void *)data); + + mutex_init(&data->lock); + err = st_stts22h_check_whoami(data); + if (err < 0) + return err; + + err = st_stts22h_init(data); + if (err < 0) + return err; + + iio_dev->name = client->name; + iio_dev->dev.parent = &client->dev; + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->info = &st_stts22h_info; + iio_dev->channels = st_stts22h_channel; + iio_dev->num_channels = ARRAY_SIZE(st_stts22h_channel); + iio_dev->available_scan_masks = st_stts22h_available_scan_masks; + data->iio_devs = iio_dev; + + /* configure hrtimer */ + data->odr = st_stts22h_odr_table.odr_avl[0].hz; + data->enable = false; + hrtimer_init(&data->hr_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + data->hr_timer.function = &st_stts22h_poll_function_read; + data->sensorktime = ktime_set(0, HZ_TO_PERIOD_NSEC(data->odr)); + INIT_WORK(&data->iio_work, st_stts22h_poll_function_work); + + err = st_stts22h_allocate_workqueue(data); + if (err < 0) + return err; + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,13,0) + err = devm_iio_kfifo_buffer_setup(data->dev, data->iio_devs, + INDIO_BUFFER_SOFTWARE, + &st_stts22h_buffer_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(data->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(data->iio_devs, buffer); + data->iio_devs->modes |= INDIO_BUFFER_SOFTWARE; + data->iio_devs->setup_ops = &st_stts22h_buffer_ops; +#endif /* LINUX_VERSION_CODE */ + + return devm_iio_device_register(data->dev, data->iio_devs); +} + +static int st_stts22h_remove(struct i2c_client *client) +{ + struct st_stts22h_data *data = dev_get_drvdata(&client->dev); + int err = 0; + + if (data->enable) + err = st_stts22h_sensor_set_enable(data, false); + + st_stts22h_flush_works(data); + destroy_workqueue(data->st_stts22h_workqueue); + data->st_stts22h_workqueue = NULL; + + return err; +} + +static int __maybe_unused st_stts22h_suspend(struct device *dev) +{ + struct st_stts22h_data *data = dev_get_drvdata(dev); + int err = 0; + + if (data->enable) + err = st_stts22h_sensor_set_enable(data, false); + + return err; +} + +static int __maybe_unused st_stts22h_resume(struct device *dev) +{ + struct st_stts22h_data *data = dev_get_drvdata(dev); + int err = 0; + + if (data->enable) + err = st_stts22h_sensor_set_enable(data, true); + + return err; +} + +#ifdef CONFIG_PM +const struct dev_pm_ops st_stts22h_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_stts22h_suspend, st_stts22h_resume) +}; +#endif /* CONFIG_PM */ + +static const struct of_device_id st_stts22h_of_match[] = { + { + .compatible = "st,stts22h", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_stts22h_of_match); + +static const struct i2c_device_id st_stts22h_id_table[] = { + { ST_STTS22H_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_stts22h_id_table); + +static struct i2c_driver st_stts22h_driver = { + .driver = { + .name = "st_stts22h_i2c", +#ifdef CONFIG_PM + .pm = &st_stts22h_pm_ops, +#endif /* CONFIG_PM */ + .of_match_table = of_match_ptr(st_stts22h_of_match), + }, + .probe = st_stts22h_probe, + .remove = st_stts22h_remove, + .id_table = st_stts22h_id_table, +}; +module_i2c_driver(st_stts22h_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("stts22h ST MEMS temperature sensor driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/stm/temperature/stts22h.h b/drivers/iio/stm/temperature/stts22h.h new file mode 100644 index 000000000000..cdd25d193bac --- /dev/null +++ b/drivers/iio/stm/temperature/stts22h.h @@ -0,0 +1,104 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics stts22h temperature driver + * + * MEMS Software Solutions Team + * + * Copyright 2021 STMicroelectronics Inc. + */ + +#ifndef ST_STTS22H_H +#define ST_STTS22H_H + +#include +#include +#include + +#define ST_STTS22H_DEV_NAME "stts22h" + +#define ST_STTS22H_WHOAMI_ADDR 0x01 +#define ST_STTS22H_WHOAMI_VAL 0xa0 + +#define ST_STTS22H_TEMP_H_LIMIT_ADDR 0x02 +#define ST_STTS22H_TEMP_L_LIMIT_ADDR 0x03 + +#define ST_STTS22H_CTRL_ADDR 0x04 +#define ST_STTS22H_LOW_ODR_START_MASK BIT(7) +#define ST_STTS22H_BDU_MASK BIT(6) +#define ST_STTS22H_AVG_MASK GENMASK(5,4) +#define ST_STTS22H_IF_ADD_INC_MASK BIT(3) +#define ST_STTS22H_FREERUN_MASK BIT(2) +#define ST_STTS22H_TIME_OUT_DIS_MASK BIT(1) +#define ST_STTS22H_ONE_SHOT_MASK BIT(0) + +#define ST_STTS22H_STATUS_ADDR 0x05 +#define ST_STTS22H_UNDER_THL_MASK BIT(2) +#define ST_STTS22H_OVER_THH_MASK BIT(1) +#define ST_STTS22H_BUSY_MASK BIT(0) + +#define ST_STTS22H_TEMP_L_OUT_ADDR 0x06 + +#define ST_STTS22H_SOFTWARE_RESET_ADDR 0x0c +#define ST_STTS22H_LOW_ODR_ENABLE_MASK BIT(6) +#define ST_STTS22H_SW_RESET_MASK BIT(1) + +#define ST_STTS22H_ODR_LIST_SIZE 4 +#define ST_STTS22H_GAIN 100 +#define ST_STTS22H_SAMPLE_SIZE sizeof(s16) + +#define HZ_TO_PERIOD_NSEC(hz) (1000000000 / \ + ((u32)(hz))) + + +/** + * struct st_stts22h_reg - Sensor data register and mask + * + * @addr: Register address. + * @mask: Bit mask. + */ +struct st_stts22h_reg { + u8 addr; + u8 mask; +}; + +/** + * struct st_stts22h_odr - Sensor data odr entry + * + * @hz: Sensor ODR. + * @val: Register value. + */ +struct st_stts22h_odr { + u8 hz; + u8 val; +}; + +/** + * struct st_stts22h_data - Sensor data instance + * + * @st_stts22h_workqueue: Temperature workqueue. + * @iio_work: Work to schedule temperature read function. + * @iio_devs: Linux Device. + * @hr_timer: Timer to schedule workeueue. + * @sensorktime: Sensor schedule timeout. + * @dev: I2C client device. + * @mutex: Mutex lock to access to device registers. + * @timestamp: Sensor timestamp. + * @enable: Enable sensor flag. + * @irq: Interrupt number (TODO). + * @odr: Sensor ODR. + */ +struct st_stts22h_data { + struct workqueue_struct *st_stts22h_workqueue; + struct work_struct iio_work; + struct iio_dev *iio_devs; + struct hrtimer hr_timer; + ktime_t sensorktime; + struct device *dev; + struct mutex lock; + s64 timestamp; + bool enable; + int irq; + u8 odr; +}; + +#endif /* ST_STTS22H_H */ diff --git a/drivers/iio/stm/tmos/Kconfig b/drivers/iio/stm/tmos/Kconfig new file mode 100644 index 000000000000..2b0f65845f71 --- /dev/null +++ b/drivers/iio/stm/tmos/Kconfig @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# IIO tmos drivers configuration +# +# When adding new entries keep the list in alphabetical order + +menu "Tmos" + +source "drivers/iio/stm/tmos/st_sths34pf80/Kconfig" + +endmenu diff --git a/drivers/iio/stm/tmos/Makefile b/drivers/iio/stm/tmos/Makefile new file mode 100644 index 000000000000..db4a0d9a0053 --- /dev/null +++ b/drivers/iio/stm/tmos/Makefile @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for Tmos +# + +# When adding new entries keep the list in alphabetical order +obj-y += st_sths34pf80/ diff --git a/drivers/iio/stm/tmos/st_sths34pf80/Kconfig b/drivers/iio/stm/tmos/st_sths34pf80/Kconfig new file mode 100644 index 000000000000..0331af2537f7 --- /dev/null +++ b/drivers/iio/stm/tmos/st_sths34pf80/Kconfig @@ -0,0 +1,25 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_STHS34PF80 + tristate "STMicroelectronics STHS34PF80 sensor" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_ST_STHS34PF80_I2C if (I2C) + select IIO_ST_STHS34PF80_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics + STHS34PF80 tmos sensor. + + To compile this driver as a module, choose M here: the module + will be called st_sths34pf80. + +config IIO_ST_STHS34PF80_I2C + tristate + select REGMAP_I2C + depends on IIO_ST_STHS34PF80 + +config IIO_ST_STHS34PF80_SPI + tristate + select REGMAP_SPI + depends on IIO_ST_STHS34PF80 diff --git a/drivers/iio/stm/tmos/st_sths34pf80/Makefile b/drivers/iio/stm/tmos/st_sths34pf80/Makefile new file mode 100644 index 000000000000..0abcc19e500e --- /dev/null +++ b/drivers/iio/stm/tmos/st_sths34pf80/Makefile @@ -0,0 +1,5 @@ +# SPDX-License-Identifier: GPL-2.0-only + +obj-$(CONFIG_IIO_ST_STHS34PF80) += st_sths34pf80_core.o +obj-$(CONFIG_IIO_ST_STHS34PF80_I2C) += st_sths34pf80_i2c.o +obj-$(CONFIG_IIO_ST_STHS34PF80_SPI) += st_sths34pf80_spi.o diff --git a/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80.h b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80.h new file mode 100644 index 000000000000..c715c9aed853 --- /dev/null +++ b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80.h @@ -0,0 +1,403 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_sths34pf80 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#ifndef ST_STHS34PF80_H +#define ST_STHS34PF80_H + +#include +#include +#include +#include +#include + +#define ST_STHS34PF80_TCOMP + +#define ST_STHS34PF80_ODR_EXPAND(odr, uodr) (((odr) * 1000000) + (uodr)) + +#define ST_STHS34PF80_DEV_NAME "sths34pf80" + +/* register map */ +#define ST_STHS34PF80_LPF1_ADDR 0x0c +#define ST_STHS34PF80_LPF_P_M_MASK GENMASK(5, 3) +#define ST_STHS34PF80_LPF_M_MASK GENMASK(2, 0) + +#define ST_STHS34PF80_LPF2_ADDR 0x0d +#define ST_STHS34PF80_LPF_P_MASK GENMASK(5, 3) +#define ST_STHS34PF80_LPF_A_T_MASK GENMASK(2, 0) + +#define ST_STHS34PF80_WHOAMI_ADDR 0x0f +#define ST_STHS34PF80_WHOAMI_VAL 0xd3 + +#define ST_STHS34PF80_AVG_TRIM_ADDR 0x10 +#define ST_STHS34PF80_AVG_T_MASK GENMASK(5, 4) +#define ST_STHS34PF80_AVG_TMOS_MASK GENMASK(2, 0) + +#define ST_STHS34PF80_SENSITIVITY_DATA_ADDR 0x1d + +#define ST_STHS34PF80_CTRL1_ADDR 0x20 +#define ST_STHS34PF80_BDU_MASK BIT(4) +#define ST_STHS34PF80_ODR_MASK GENMASK(3, 0) + +#define ST_STHS34PF80_CTRL2_ADDR 0x21 +#define ST_STHS34PF80_BOOT_MASK BIT(7) +#define ST_STHS34PF80_FUNC_CFG_ACCESS_MASK BIT(4) +#define ST_STHS34PF80_ONE_SHOT_MASK BIT(0) + +#define ST_STHS34PF80_CTRL3_ADDR 0x22 +#define ST_STHS34PF80_INT_H_L_MASK BIT(7) +#define ST_STHS34PF80_PP_OD_MASK BIT(6) +#define ST_STHS34PF80_INT_MSK_MASK GENMASK(5, 3) +#define ST_STHS34PF80_INT_MSK2_MASK BIT(5) +#define ST_STHS34PF80_INT_MSK1_MASK BIT(4) +#define ST_STHS34PF80_INT_MSK0_MASK BIT(3) +#define ST_STHS34PF80_INT_LATCHED_MASK BIT(2) +#define ST_STHS34PF80_IEN_MASK GENMASK(1, 0) + +#define ST_STHS34PF80_IEN_DRDY_VAL 0x01 +#define ST_STHS34PF80_IEN_INT_OR_VAL 0x02 + +#define ST_STHS34PF80_STATUS_ADDR 0x23 +#define ST_STHS34PF80_DRDY_MASK BIT(2) + +#define ST_STHS34PF80_FUNC_STATUS_ADDR 0x25 +#define ST_STHS34PF80_PRES_FLAG_MASK BIT(2) +#define ST_STHS34PF80_MOT_FLAG_MASK BIT(1) +#define ST_STHS34PF80_TAMB_SHOCK_FLAG_MASK BIT(0) + +#define ST_STHS34PF80_TOBJECT_L_ADDR 0x26 +#define ST_STHS34PF80_TAMBIENT_L_ADDR 0x28 + +#ifdef ST_STHS34PF80_TCOMP +#define ST_STHS34PF80_TOBJECT_COMP_L_ADDR 0x38 +#endif /* ST_STHS34PF80_TCOMP */ + +#define ST_STHS34PF80_TPRESENCE_L_ADDR 0x3a +#define ST_STHS34PF80_TMOTION_L_ADDR 0x3c +#define ST_STHS34PF80_TAMB_SHOCK_L_ADDR 0x3e + +/* embedded functions register map */ +#define ST_STHS34PF80_FUNC_CFG_ADDR_ADDR 0x08 +#define ST_STHS34PF80_FUNC_CFG_DATA_ADDR 0x09 + +#define ST_STHS34PF80_PAGE_RW_ADDR 0x11 +#define ST_STHS34PF80_FUNC_CFG_WRITE_MASK BIT(6) +#define ST_STHS34PF80_FUNC_CFG_READ_MASK BIT(5) + +#define ST_STHS34PF80_PRESENCE_THS_ADDR 0x20 +#define ST_STHS34PF80_MOTION_THS_ADDR 0x22 +#define ST_STHS34PF80_TAMB_SHOCK_THS_ADDR 0x24 +#define ST_STHS34PF80_HYST_MOTION_ADDR 0x26 +#define ST_STHS34PF80_HYST_PRESENCE_ADDR 0x27 + +#define ST_STHS34PF80_ALGO_CONFIG_ADDR 0x28 +#define ST_STHS34PF80_INT_PULSED_MASK BIT(3) +#define ST_STHS34PF80_COMP_TYPE_MASK BIT(2) +#define ST_STHS34PF80_SEL_ABS_MASK BIT(1) + +#define ST_STHS34PF80_HYST_TAMB_SHOCK_ADDR 0x29 + +#define ST_STHS34PF80_RESET_ALGO_ADDR 0x2a +#define ST_STHS34PF80_ALGO_ENABLE_RESET_MASK BIT(0) + +/* default values */ +#define ST_STHS34PF80_TOBJECT_GAIN 2000 + +#ifdef ST_STHS34PF80_TCOMP +#define ST_STHS34PF80_TOBJECT_COMP_GAIN 2000 +#endif /* ST_STHS34PF80_TCOMP */ + +#define ST_STHS34PF80_TAMBIENT_GAIN 100 + +#define ST_STHS34PF80_LPF_M_DEFAULT 0x04 +#define ST_STHS34PF80_LPF_P_M_DEFAULT 0x00 +#define ST_STHS34PF80_LPF_P_DEFAULT 0x04 +#define ST_STHS34PF80_LPF_A_T_DEFAULT 0x02 + +#define ST_STHS34PF80_PRESENCE_THS_DEFAULT 0x00c8 +#define ST_STHS34PF80_MOTION_THS_DEFAULT 0x00c8 +#define ST_STHS34PF80_TAMB_SHOCK_THS_DEFAULT 0x000a + +#define ST_STHS34PF80_HYST_MOTION_DEFAULT 0x32 +#define ST_STHS34PF80_HYST_PRESENCE_DEFAULT 0x32 + +#define ST_STHS34PF80_DATA_CHANNEL(chan_type, addr, mod, \ + ch2, scan_idx, rb, sb, sg) \ +{ \ + .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, \ + }, \ +} + +static const struct iio_event_spec st_sths34pf80_thr_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), +}; + +#define ST_STHS34PF80_EVENT_CHANNEL(ctype) \ +{ \ + .type = ctype, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_sths34pf80_thr_event, \ + .num_event_specs = 1, \ +} + +#define ST_STHS34PF80_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +/** + * struct st_sths34pf80_reg - Generic sensor register + * description (addr + mask) + * + * @addr: Address of register. + * @mask: Bitmask register for proper usage. + */ +struct st_sths34pf80_reg { + u8 addr; + u8 mask; +}; + +/** + * struct st_sths34pf80_odr - Single ODR entry + * @hz: Most significant part of the sensor ODR (Hz). + * @uhz: Less significant part of the sensor ODR (micro Hz). + * @val: ODR register value. + * @avg: Average suggested for this ODR. + */ +struct st_sths34pf80_odr { + u16 hz; + u32 uhz; + u8 val; + u8 avg; +}; + +/** + * struct st_sths34pf80_fs + * brief Full scale entry + * + * @gain: The gain to obtain data value from raw data (LSB). + * @val: Register value. + */ +struct st_sths34pf80_fs { + u32 gain; + u8 val; +}; + +/** + * struct st_sths34pf80_lpf + * brief Low pass filter setting + * + * @reg: Register to update LPF bandwidth. + * @mask: Register bitmask. + * @val: Register value. + */ +struct st_sths34pf80_lpf { + u8 reg; + u8 mask; + u8 val; +}; + +/** + * struct st_sths34pf80_threshold + * brief Thresholds setting + * + * @reg: Register to update sensor threshold. + * @val: Register value. + */ +struct st_sths34pf80_threshold { + u8 reg; + u16 val; +}; + +/** + * struct st_sths34pf80_hysteresis + * brief hysteresis setting + * + * @reg: Register to update sensor hysteresis. + * @val: Register value. + */ +struct st_sths34pf80_hysteresis { + u8 reg; + u8 val; +}; + +enum st_sths34pf80_sensor_id { + ST_STHS34PF80_ID_TAMB_OBJ = 0, +#ifdef ST_STHS34PF80_TCOMP + ST_STHS34PF80_ID_TOBJECT_COMP, +#endif /* ST_STHS34PF80_TCOMP */ + ST_STHS34PF80_ID_TAMB_SHOCK, + ST_STHS34PF80_ID_TMOTION, + ST_STHS34PF80_ID_TPRESENCE, + ST_STHS34PF80_ID_MAX, +}; + +/** + * struct st_sths34pf80_sensor - ST TMOS sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_sths34pf80_hw. + * @odr: Output data rate of the sensor [Hz]. + * @uodr: Output data rate of the sensor [uHz]. + * @lpf: Sensor low pass filter settings. + * @threshold: Sensor thresholds. + * @hysteresis: Sensor hysteresis. + */ +struct st_sths34pf80_sensor { + char name[32]; + + enum st_sths34pf80_sensor_id id; + struct st_sths34pf80_hw *hw; + + int odr; + int uodr; + + struct st_sths34pf80_lpf lpf; + struct st_sths34pf80_threshold threshold; + struct st_sths34pf80_hysteresis hysteresis; +}; + +/** + * struct st_sths34pf80_hw - ST TMOS MEMS hw instance + * @dev_name: STM device name. + * @dev: Pointer to instance of struct device (I2C or SPI). + * @irq: Device interrupt line (I2C or SPI). + * @regmap: Register map of the device. + * @page_lock: Mutex to prevent concurrent access to the page selector. + * @int_lock: Mutex to prevent concurrent access to interrupt configuration. + * @state: hw operational state. + * @enable_mask: Enabled sensor bitmask. + * @event_mask: Enabled event generation bitmask. + * @vdd_supply: Voltage regulator for VDD. + * @vddio_supply: Voltage regulator for VDDIIO. + * @iio_devs: Pointers to iio_dev instances. + * @ts: Event hardware timestamp. + * @tcomp: Temperature compensation enabled flag. + * @edge_trigger: Interrupt configuration type. + * @sensitivity: Data sensitivity (for tobj compensated only) + */ +struct st_sths34pf80_hw { + char dev_name[16]; + struct device *dev; + int irq; + struct regmap *regmap; + struct mutex page_lock; + struct mutex int_lock; + + unsigned long state; + u64 enable_mask; + u64 event_mask; + + struct regulator *vdd_supply; + struct regulator *vddio_supply; + + struct iio_dev *iio_devs[ST_STHS34PF80_ID_MAX]; + + s64 ts; + +#ifdef ST_STHS34PF80_TCOMP + bool tcomp; +#endif /* ST_STHS34PF80_TCOMP */ + + bool edge_trigger; + + u8 sensitivity; +}; + +static inline int +__st_sths34pf80_write_with_mask(struct st_sths34pf80_hw *hw, + unsigned int addr, + unsigned int mask, + unsigned int data) +{ + int err; + unsigned int val = ST_STHS34PF80_SHIFT_VAL(data, mask); + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + return err; +} + +static inline int +st_sths34pf80_update_bits_locked(struct st_sths34pf80_hw *hw, + unsigned int addr, unsigned int mask, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = __st_sths34pf80_write_with_mask(hw, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_sths34pf80_read_locked(struct st_sths34pf80_hw *hw, + unsigned int addr, void *val, + unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_sths34pf80_write_locked(struct st_sths34pf80_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_sths34pf80_set_page_access(struct st_sths34pf80_hw *hw, + unsigned int val) +{ + return regmap_update_bits(hw->regmap, + ST_STHS34PF80_CTRL2_ADDR, + ST_STHS34PF80_FUNC_CFG_ACCESS_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_ACCESS_MASK, val)); +} + +static inline s64 st_sths34pf80_get_time_ns(struct st_sths34pf80_hw *hw) +{ + return iio_get_time_ns(hw->iio_devs[ST_STHS34PF80_ID_TAMB_OBJ]); +} + +int st_sths34pf80_probe(struct device *dev, int irq, + struct regmap *regmap); +int st_sths34pf80_remove(struct device *dev); + +#ifdef CONFIG_PM_SLEEP +extern const struct dev_pm_ops st_sths34pf80_pm_ops; +#endif /* CONFIG_PM_SLEEP */ +#endif /* ST_STHS34PF80_H */ diff --git a/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_core.c b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_core.c new file mode 100644 index 000000000000..e12a85a981a5 --- /dev/null +++ b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_core.c @@ -0,0 +1,2157 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_sths34pf80 sensor driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_sths34pf80.h" + +static int st_sths34pf80_write_cfg(struct st_sths34pf80_hw *hw, + int add, u8 *buffer, int len); +/** + * struct st_sths34pf80_odr_table_entry - Sensor ODR table + * @size: Size of ODR table. + * @reg: ODR register. + * @odr_avl: Array of supported ODR values. + */ +static const struct st_sths34pf80_odr_table_entry { + u8 size; + struct st_sths34pf80_reg reg; + struct st_sths34pf80_odr odr_avl[9]; +} st_sths34pf80_odr_table = { + .size = 9, + .reg = { + .addr = ST_STHS34PF80_CTRL1_ADDR, + .mask = ST_STHS34PF80_ODR_MASK, + }, + .odr_avl = { + { 0, 0, 0x00, 0x03 }, + { 0, 250000, 0x01, 0x03 }, + { 0, 500000, 0x02, 0x03 }, + { 1, 0, 0x03, 0x03 }, + { 2, 0, 0x04, 0x03 }, + { 4, 0, 0x05, 0x03 }, + { 8, 0, 0x06, 0x03 }, + { 15, 0, 0x07, 0x02 }, + { 30, 0, 0x08, 0x02 }, + }, +}; + +/** + * @brief TAmbient and TObject IIO channels description + * + * TAmbient and TObject exports to IIO framework the following channels: + * 1) tobject - temperature (milli celsius) + * 2) tambient - temperature (milli celsius) + * 3) timestamp (ns) + */ +static const struct iio_chan_spec st_sths34pf80_tobj_amb_channels[] = { + ST_STHS34PF80_DATA_CHANNEL(IIO_TEMP, + ST_STHS34PF80_TOBJECT_L_ADDR, 1, + IIO_MOD_TEMP_OBJECT, + 0, 16, 16, 's'), + ST_STHS34PF80_DATA_CHANNEL(IIO_TEMP, + ST_STHS34PF80_TAMBIENT_L_ADDR, 1, + IIO_MOD_TEMP_AMBIENT, + 1, 16, 16, 's'), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +#ifdef ST_STHS34PF80_TCOMP +/** + * @brief TObject compensated IIO channels description + * + * TObject compensated exports to IIO framework the following channels: + * 1) TObject - temperature (milli celsius) + * 2) timestamp (ns) + */ +static const struct iio_chan_spec st_sths34pf80_tobject_comp_channels[] = { + ST_STHS34PF80_DATA_CHANNEL(IIO_TEMP, + ST_STHS34PF80_TOBJECT_COMP_L_ADDR, 1, + IIO_MOD_TEMP_OBJECT, 0, 16, 16, 's'), + IIO_CHAN_SOFT_TIMESTAMP(1), +}; +#endif /* ST_STHS34PF80_TCOMP */ + +/** + * @brief TMotion IIO channels description + * + * TMotion exports to IIO framework the following channels: + * 1) tmotion + * 2) tmotion flag + * 2) timestamp (ns) + */ +static const struct iio_chan_spec st_sths34pf80_tmotion_channels[] = { + ST_STHS34PF80_DATA_CHANNEL(IIO_TEMP, + ST_STHS34PF80_TMOTION_L_ADDR, 1, + IIO_MOD_X, + 0, 16, 16, 's'), + ST_STHS34PF80_DATA_CHANNEL(IIO_PROXIMITY, + ST_STHS34PF80_FUNC_STATUS_ADDR, 1, + IIO_MOD_TEMP_OBJECT, + 1, 8, 8, 's'), + ST_STHS34PF80_EVENT_CHANNEL(IIO_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +/** + * @brief TPresence IIO channels description + * + * TPresence exports to IIO framework the following data channels: + * 1) tpresence + * 2) tpresence flag + * 3) timestamp (ns) + */ +static const struct iio_chan_spec st_sths34pf80_tpresence_channels[] = { + ST_STHS34PF80_DATA_CHANNEL(IIO_TEMP, + ST_STHS34PF80_TPRESENCE_L_ADDR, 1, + IIO_MOD_Y, + 0, 16, 16, 's'), + ST_STHS34PF80_DATA_CHANNEL(IIO_PROXIMITY, + ST_STHS34PF80_FUNC_STATUS_ADDR, 1, + IIO_MOD_TEMP_OBJECT, + 1, 8, 8, 's'), + ST_STHS34PF80_EVENT_CHANNEL(IIO_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +/** + * @brief TAmbshock IIO channels description + * + * TAmbshock exports to IIO framework the following channels: + * 1) tambshock + * 2) tambshock flag + * 3) timestamp (ns) + */ +static const struct iio_chan_spec st_sths34pf80_tambshock_channels[] = { + ST_STHS34PF80_DATA_CHANNEL(IIO_TEMP, + ST_STHS34PF80_TAMB_SHOCK_L_ADDR, 1, + IIO_MOD_Z, + 0, 16, 16, 's'), + ST_STHS34PF80_DATA_CHANNEL(IIO_PROXIMITY, + ST_STHS34PF80_FUNC_STATUS_ADDR, 1, + IIO_MOD_TEMP_OBJECT, + 1, 8, 8, 's'), + ST_STHS34PF80_EVENT_CHANNEL(IIO_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(2), +}; + +static __maybe_unused int +st_sths34pf80_reg_access(struct iio_dev *iio_dev, + unsigned int reg, unsigned int writeval, + unsigned int *readval) +{ + struct st_sths34pf80_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; +} + +/** + * st_sths34pf80_check_whoami - Check value of HW Device ID + * + * @param hw: ST TMOS MEMS sensr hw structure. + * @return 0 if OK, negative value for ERROR. + */ +static int st_sths34pf80_check_whoami(struct st_sths34pf80_hw *hw) +{ + int err, data; + + err = regmap_read(hw->regmap, ST_STHS34PF80_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + + return err; + } + + if (data != ST_STHS34PF80_WHOAMI_VAL) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + + return -ENODEV; + } + + return 0; +} + +static int +st_sths34pf80_get_odr_val(int odr, int uodr, + const struct st_sths34pf80_odr **oe) +{ + int req_odr = ST_STHS34PF80_ODR_EXPAND(odr, uodr); + int sensor_odr; + int i; + + for (i = 0; i < st_sths34pf80_odr_table.size; i++) { + sensor_odr = ST_STHS34PF80_ODR_EXPAND( + st_sths34pf80_odr_table.odr_avl[i].hz, + st_sths34pf80_odr_table.odr_avl[i].uhz); + if (sensor_odr >= req_odr) { + *oe = &st_sths34pf80_odr_table.odr_avl[i]; + + return 0; + } + } + + return -EINVAL; +} + +static u16 +st_sths34pf80_check_odr_dependency(struct st_sths34pf80_hw *hw, + int odr, int uodr, + enum st_sths34pf80_sensor_id ref_id, + enum st_sths34pf80_sensor_id id) +{ + struct st_sths34pf80_sensor *ref = iio_priv(hw->iio_devs[ref_id]); + struct st_sths34pf80_sensor *orig = iio_priv(hw->iio_devs[id]); + bool enable = odr > 0; + u16 ret; + + if (enable) { + if (hw->enable_mask & BIT(ref_id)) + ret = max_t(u16, ref->odr, odr); + else + ret = odr; + } else { + if (hw->enable_mask & BIT(id) && hw->event_mask & BIT(id)) + return orig->odr; + + ret = (hw->enable_mask & BIT(ref_id) || + hw->event_mask & BIT(ref_id)) ? ref->odr : 0; + } + + return ret; +} + +static int st_sths34pf80_set_odr(struct st_sths34pf80_sensor *sensor, + int req_odr, int req_uodr) +{ + enum st_sths34pf80_sensor_id id = sensor->id; + struct st_sths34pf80_hw *hw = sensor->hw; + const struct st_sths34pf80_odr *oe; + int ret; + u8 updt; + + switch (id) { + case ST_STHS34PF80_ID_TAMB_OBJ: + +#ifdef ST_STHS34PF80_TCOMP + case ST_STHS34PF80_ID_TOBJECT_COMP: +#endif /* ST_STHS34PF80_TCOMP */ + + case ST_STHS34PF80_ID_TAMB_SHOCK: + case ST_STHS34PF80_ID_TMOTION: + case ST_STHS34PF80_ID_TPRESENCE: { + int odr; + int i; + + id = ST_STHS34PF80_ID_TAMB_OBJ; + for (i = ST_STHS34PF80_ID_TAMB_OBJ; + i < ST_STHS34PF80_ID_MAX; i++) { + if (!hw->iio_devs[i] || i == sensor->id) + continue; + + odr = st_sths34pf80_check_odr_dependency(hw, req_odr, + req_uodr, i, + sensor->id); + if (odr != req_odr) + return 0; + } + break; + } + default: + break; + } + + ret = st_sths34pf80_get_odr_val(req_odr, req_uodr, &oe); + if (ret) + return ret; + + /* update avg tmos bitfield accordingly to new odr */ + ret = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_AVG_TRIM_ADDR, + ST_STHS34PF80_AVG_TMOS_MASK, + oe->avg); + if (ret < 0) + return ret; + + ret = st_sths34pf80_update_bits_locked(hw, + st_sths34pf80_odr_table.reg.addr, + st_sths34pf80_odr_table.reg.mask, + oe->val); + if (ret < 0) + return ret; + + /* reset algos when in power down mode (odr = 0) */ + if (oe->val == 0) { + updt = ST_STHS34PF80_ALGO_ENABLE_RESET_MASK; + ret = st_sths34pf80_write_cfg(hw, ST_STHS34PF80_RESET_ALGO_ADDR, + &updt, 1); + } + + return ret < 0 ? ret : 0; +} + +/** + * st_sths34pf80_manage_interrupt_cfg - Configure interrupts + * + * @param sensor: IIO event sensor. + * @param state: New event state. + * @return 0 if OK, negative for ERROR + */ +static int +st_sths34pf80_manage_interrupt_cfg(struct st_sths34pf80_sensor *sensor, + bool new_ev_state, bool new_en_state) +{ + enum st_sths34pf80_sensor_id id = sensor->id; + struct st_sths34pf80_hw *hw = sensor->hw; + int err = 0; + + mutex_lock(&hw->int_lock); + + if ((hw->event_mask & BIT(id)) != new_ev_state) { + if (!hw->event_mask && !hw->enable_mask && new_ev_state) { + /* set data ready pulsed */ + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_LATCHED_MASK, + 0); + if (err < 0) + goto unlock; + + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_IEN_MASK, + ST_STHS34PF80_IEN_INT_OR_VAL); + } else if (((hw->event_mask & BIT(id)) == hw->event_mask) && + !new_ev_state) { + if (!hw->edge_trigger) { + /* set data ready latched */ + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_LATCHED_MASK, + 1); + if (err < 0) + goto unlock; + } + + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_IEN_MASK, + hw->enable_mask ? + ST_STHS34PF80_IEN_DRDY_VAL : 0); + } + } + + if ((hw->enable_mask & BIT(id)) != new_en_state) { + if (new_en_state) { + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_IEN_MASK, + ST_STHS34PF80_IEN_DRDY_VAL); + if (err < 0) + goto unlock; + + /* set data ready latched */ + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_LATCHED_MASK, + 1); + if (err < 0) + goto unlock; + } else { + if ((hw->enable_mask) == BIT(id)) { + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_IEN_MASK, + hw->event_mask ? + ST_STHS34PF80_IEN_INT_OR_VAL : 0); + if (err < 0) + goto unlock; + + /* reset data ready latched */ + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_LATCHED_MASK, + 0); + if (err < 0) + goto unlock; + } + } + } + +unlock: + mutex_unlock(&hw->int_lock); + + return err < 0 ? err : 0; +} + +static int +st_sths34pf80_sensor_set_enable(struct st_sths34pf80_sensor *sensor, + bool enable) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_sths34pf80_set_odr(sensor, odr, uodr); + if (err < 0) + return err; + + err = st_sths34pf80_manage_interrupt_cfg(sensor, + !!(hw->event_mask & BIT(sensor->id)), + enable); + if (err) + return err; + + if (enable) + hw->enable_mask |= BIT(sensor->id); + else + hw->enable_mask &= ~BIT(sensor->id); + + return 0; +} + +static int +st_sths34pf80_event_set_enable(struct st_sths34pf80_sensor *sensor, + bool enable) +{ + int uodr = enable ? sensor->uodr : 0; + int odr = enable ? sensor->odr : 0; + int err; + + err = st_sths34pf80_set_odr(sensor, odr, uodr); + if (err < 0) + return err; + + if (enable) + sensor->hw->event_mask |= BIT(sensor->id); + else + sensor->hw->event_mask &= ~BIT(sensor->id); + + return 0; +} + +/** + * st_sths34pf80_write_cfg - write buffer to embedded function registers + * + * @param hw: ST TMOS MEMS hw instance. + * @param add: Starting embedded function address register. + * @param buffer: Buffer data to write to embedded function registers. + * @param len: Buffer data len. + * @return 0 if OK, negative for ERROR + */ +static int st_sths34pf80_write_cfg(struct st_sths34pf80_hw *hw, + int add, u8 *buffer, int len) +{ + int err, i; + + /* enable access to embedded function registers */ + err = regmap_update_bits(hw->regmap, + ST_STHS34PF80_CTRL2_ADDR, + ST_STHS34PF80_FUNC_CFG_ACCESS_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_ACCESS_MASK, + 1)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, ST_STHS34PF80_PAGE_RW_ADDR, + ST_STHS34PF80_FUNC_CFG_WRITE_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_WRITE_MASK, 1)); + if (err < 0) + goto out_err; + + err = regmap_write(hw->regmap, + ST_STHS34PF80_FUNC_CFG_ADDR_ADDR, add); + if (err < 0) + goto out_err; + + /* loop on write, by default auto increments is enabled */ + for (i = 0; i < len; i++) { + err = regmap_write(hw->regmap, + ST_STHS34PF80_FUNC_CFG_DATA_ADDR, + buffer[i]); + if (err < 0) + goto out_err; + } + + err = regmap_update_bits(hw->regmap, ST_STHS34PF80_PAGE_RW_ADDR, + ST_STHS34PF80_FUNC_CFG_WRITE_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_WRITE_MASK, 0)); + +out_err: + regmap_write(hw->regmap, ST_STHS34PF80_CTRL2_ADDR, 0); + + return err < 0 ? err : 0; +} + +/** + * st_sths34pf80_read_cfg - read embedded function register + * + * @param hw: ST TMOS MEMS hw instance. + * @param add: Starting embedded function address register. + * @param val: Buffer data to write to embedded function registers. + * @return 0 if OK, negative for ERROR + */ +static int __maybe_unused st_sths34pf80_read_cfg(struct st_sths34pf80_hw *hw, + int add, u8 *val) +{ + unsigned int rval; + int err; + + /* enable access to embedded function registers */ + err = regmap_update_bits(hw->regmap, + ST_STHS34PF80_CTRL2_ADDR, + ST_STHS34PF80_FUNC_CFG_ACCESS_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_ACCESS_MASK, 1)); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, ST_STHS34PF80_PAGE_RW_ADDR, + ST_STHS34PF80_FUNC_CFG_READ_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_READ_MASK, 1)); + if (err < 0) + goto out_err; + + err = regmap_write(hw->regmap, ST_STHS34PF80_FUNC_CFG_ADDR_ADDR, add); + if (err < 0) + goto out_err; + + err = regmap_read(hw->regmap, + ST_STHS34PF80_FUNC_CFG_DATA_ADDR, + &rval); + if (err < 0) + goto out_err; + + *val = (u8)rval; + + err = regmap_update_bits(hw->regmap, ST_STHS34PF80_PAGE_RW_ADDR, + ST_STHS34PF80_FUNC_CFG_READ_MASK, + FIELD_PREP(ST_STHS34PF80_FUNC_CFG_READ_MASK, 0)); + +out_err: + regmap_write(hw->regmap, ST_STHS34PF80_CTRL2_ADDR, 0); + + return err < 0 ? err : 0; +} + +/** + * st_sths34pf80_update_cfg - update embedded function register + * + * @param hw: ST TMOS MEMS hw instance. + * @param add: Embedded function address register. + * @param mask: Data mask. + * @param val: Data to update to embedded function registers. + * @return 0 if OK, negative for ERROR + */ +static int st_sths34pf80_update_cfg(struct st_sths34pf80_hw *hw, + int add, u8 mask, u8 val) +{ + u8 data; + int err; + + err = st_sths34pf80_read_cfg(hw, add, &data); + if (err < 0) + return err; + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + return st_sths34pf80_write_cfg(hw, add, &data, 1); +} + +static int st_sths34pf80_reset_algos(struct st_sths34pf80_hw *hw) +{ + int err, odr; + u8 updt; + + /* disable odr */ + err = regmap_bulk_read(hw->regmap, + st_sths34pf80_odr_table.reg.addr, + &odr, 1); + if (err) + return err; + + err = __st_sths34pf80_write_with_mask(hw, + st_sths34pf80_odr_table.reg.addr, + st_sths34pf80_odr_table.reg.mask, + 0); + if (err) + return err; + + updt = ST_STHS34PF80_ALGO_ENABLE_RESET_MASK; + err = st_sths34pf80_write_cfg(hw, ST_STHS34PF80_RESET_ALGO_ADDR, + &updt, 1); + if (err) + return err; + + /* restore odr */ + err = regmap_write(hw->regmap, st_sths34pf80_odr_table.reg.addr, odr); + + return err < 0 ? err : 0; +} + +#ifdef ST_STHS34PF80_TCOMP +static int st_sths34pf80_enable_comp(struct st_sths34pf80_hw *hw, + bool enable) +{ + u8 val; + int err; + int odr; + + mutex_lock(&hw->page_lock); + + /* disable odr */ + err = regmap_bulk_read(hw->regmap, + st_sths34pf80_odr_table.reg.addr, + &odr, 1); + if (err) + goto out_unlock; + + err = __st_sths34pf80_write_with_mask(hw, + st_sths34pf80_odr_table.reg.addr, + st_sths34pf80_odr_table.reg.mask, + 0); + if (err) + goto out_unlock; + + err = st_sths34pf80_update_cfg(hw, ST_STHS34PF80_ALGO_CONFIG_ADDR, + ST_STHS34PF80_COMP_TYPE_MASK, + enable ? 1 : 0); + if (err) + goto out_unlock; + + val = ST_STHS34PF80_ALGO_ENABLE_RESET_MASK; + err = st_sths34pf80_write_cfg(hw, ST_STHS34PF80_RESET_ALGO_ADDR, + &val, 1); + if (err < 0) + goto out_unlock; + + hw->tcomp = enable; + +out_unlock: + /* restore odr */ + err = regmap_write(hw->regmap, st_sths34pf80_odr_table.reg.addr, odr); + mutex_unlock(&hw->page_lock); + + return err; +} +#endif /* ST_STHS34PF80_TCOMP */ + +static int +st_sths34pf80_read_oneshot(struct st_sths34pf80_sensor *sensor, + struct iio_chan_spec const *ch, int *val) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + enum st_sths34pf80_sensor_id id = sensor->id; + u8 addr = ch->address; + int err, delay; + __le16 data; + u8 status; + + err = st_sths34pf80_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + switch (id) { + case ST_STHS34PF80_ID_TAMB_OBJ: + +#ifdef ST_STHS34PF80_TCOMP + case ST_STHS34PF80_ID_TOBJECT_COMP: +#endif /* ST_STHS34PF80_TCOMP */ + + /* consider also ODRs < 1 Hz */ + if (sensor->odr > 0) + delay = 1000000 / sensor->odr; + else + delay = 1000000 * (1000000 / sensor->uodr); + + usleep_range(delay, (delay >> 1) + delay); + + err = st_sths34pf80_read_locked(hw, addr, + &data, sizeof(data)); + if (err < 0) + goto disable; + + *val = (s16)le16_to_cpu(data); + break; + case ST_STHS34PF80_ID_TAMB_SHOCK: + case ST_STHS34PF80_ID_TMOTION: + case ST_STHS34PF80_ID_TPRESENCE: + /* consider also ODRs < 1 Hz */ + if (sensor->odr > 0) + delay = 3000000 / sensor->odr; + else + delay = 3000000 * (1000000 / sensor->uodr); + + usleep_range(delay, (delay >> 1) + delay); + + if (ch->scan_index == 0) { + err = st_sths34pf80_read_locked(hw, addr, &data, + sizeof(data)); + if (err < 0) + goto disable; + + *val = (s16)le16_to_cpu(data); + } else { + err = st_sths34pf80_read_locked(hw, addr, &status, + sizeof(status)); + if (err < 0) + goto disable; + + status = (status & (1 << (id - ST_STHS34PF80_ID_TAMB_SHOCK))) ? 1 : 0; + *val = (int)status; + } + break; + default: + break; + } +disable: + st_sths34pf80_sensor_set_enable(sensor, false); + + return err < 0 ? err : IIO_VAL_INT; +} + +static int st_sths34pf80_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_sths34pf80_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_sths34pf80_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_OFFSET: + switch (ch->type) { + case IIO_TEMP: + *val = 0; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + 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: + switch (sensor->id) { + case ST_STHS34PF80_ID_TAMB_OBJ: + *val = 1000; + ret = IIO_VAL_FRACTIONAL; + + if (ch->channel2 == IIO_MOD_TEMP_OBJECT) + *val2 = ST_STHS34PF80_TOBJECT_GAIN; + else + *val2 = ST_STHS34PF80_TAMBIENT_GAIN; + + break; +#ifdef ST_STHS34PF80_TCOMP + case ST_STHS34PF80_ID_TOBJECT_COMP: + *val = 1; + *val2 = ST_STHS34PF80_TOBJECT_COMP_GAIN; + ret = IIO_VAL_FRACTIONAL; + break; +#endif /* ST_STHS34PF80_TCOMP */ + case ST_STHS34PF80_ID_TAMB_SHOCK: + *val = 1; + *val2 = 0; + ret = IIO_VAL_INT; + break; + case ST_STHS34PF80_ID_TMOTION: + *val = 1; + *val2 = 0; + ret = IIO_VAL_INT; + break; + case ST_STHS34PF80_ID_TPRESENCE: + *val = 1; + *val2 = 0; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + break; + case IIO_PROXIMITY: + switch (sensor->id) { + case ST_STHS34PF80_ID_TAMB_SHOCK: + *val = 1; + *val2 = 0; + ret = IIO_VAL_INT; + break; + case ST_STHS34PF80_ID_TMOTION: + *val = 1; + *val2 = 0; + ret = IIO_VAL_INT; + break; + case ST_STHS34PF80_ID_TPRESENCE: + *val = 1; + *val2 = 0; + ret = IIO_VAL_INT; + break; + default: + return -EINVAL; + } + break; + default: + return -EINVAL; + } + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_sths34pf80_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_sths34pf80_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + const struct st_sths34pf80_odr *oe; + + err = st_sths34pf80_get_odr_val(val, val2, &oe); + if (!err) { + sensor->odr = oe->hz; + sensor->uodr = oe->uhz; + } + break; + } + default: + err = -EINVAL; + break; + } + + mutex_unlock(&iio_dev->mlock); + + return err; +} + +/** + * st_sths34pf80_event_sensor_enable - Enable event sensor + * + * @param sensor: IIO event sensor. + * @param state: New event state. + * @return 0 if OK, negative for ERROR + */ +static int +st_sths34pf80_event_sensor_enable(struct st_sths34pf80_sensor *sensor, + bool state) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + u8 int_mask = 0; + int err; + + if (!!(hw->event_mask & BIT(sensor->id)) == state) + return 0; + + switch (sensor->id) { + case ST_STHS34PF80_ID_TAMB_OBJ: + +#ifdef ST_STHS34PF80_TCOMP + case ST_STHS34PF80_ID_TOBJECT_COMP: +#endif /* ST_STHS34PF80_TCOMP */ + + return -EINVAL; + case ST_STHS34PF80_ID_TAMB_SHOCK: + int_mask = ST_STHS34PF80_INT_MSK0_MASK; + break; + case ST_STHS34PF80_ID_TMOTION: + int_mask = ST_STHS34PF80_INT_MSK1_MASK; + break; + case ST_STHS34PF80_ID_TPRESENCE: + int_mask = ST_STHS34PF80_INT_MSK2_MASK; + break; + default: + return -ENODEV; + } + + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + int_mask, state ? 1 : 0); + if (err) + return err; + + err = st_sths34pf80_manage_interrupt_cfg(sensor, state, + !!(hw->enable_mask & BIT(sensor->id))); + if (err) + return err; + + return st_sths34pf80_event_set_enable(sensor, state); +} + +/** + * st_sths34pf80_read_event_config - Read sensor event configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param type: Event Type. + * @param dir: Event Direction. + * @return 1 if Enabled, 0 Disabled + */ +static int +st_sths34pf80_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_sths34pf80_sensor *sensor = iio_priv(iio_dev); + struct st_sths34pf80_hw *hw = sensor->hw; + int ret; + + mutex_lock(&iio_dev->mlock); + ret = !!(hw->event_mask & BIT(sensor->id)); + mutex_unlock(&iio_dev->mlock); + + return ret; +} + +/** + * st_sths34pf80_write_event_config - Write sensor event configuration + * + * @param iio_dev: IIO Device. + * @param chan: IIO Channel. + * @param type: Event Type. + * @param dir: Event Direction. + * @param state: New event state. + * @return 0 if OK, negative for ERROR + */ +static int +st_sths34pf80_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_sths34pf80_sensor *sensor = iio_priv(iio_dev); + int err; + + mutex_lock(&iio_dev->mlock); + err = st_sths34pf80_event_sensor_enable(sensor, state); + mutex_unlock(&iio_dev->mlock); + + return err; +} + +static ssize_t +st_sths34pf80_sysfs_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + + for (i = 0; i < st_sths34pf80_odr_table.size; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ", + st_sths34pf80_odr_table.odr_avl[i].hz, + st_sths34pf80_odr_table.odr_avl[i].uhz); + } + + buf[len - 1] = '\n'; + + return len; +} + +static void __maybe_unused +st_sths34pf80_report_event(struct st_sths34pf80_sensor *sensor, + u8 *tmp, int64_t timestamp) +{ + u8 iio_buf[ALIGN(2, sizeof(s64)) + sizeof(s64) + sizeof(s64)]; + struct iio_dev *iio_dev = sensor->hw->iio_devs[sensor->id]; + + memcpy(iio_buf, tmp, 2); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, timestamp); +} + +static void +st_sths34pf80_report_2event(struct st_sths34pf80_sensor *sensor, + u8 *tmp, int64_t timestamp) +{ + u8 iio_buf[ALIGN(3, sizeof(s64)) + sizeof(s64) + sizeof(s64)]; + struct iio_dev *iio_dev = sensor->hw->iio_devs[sensor->id]; + + memcpy(iio_buf, tmp, 4); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, timestamp); +} + +static void +st_sths34pf80_report_algo_event(struct st_sths34pf80_sensor *sensor, + u8 *tmp, int64_t timestamp) +{ + u8 iio_buf[ALIGN(3, sizeof(s64)) + sizeof(s64)]; + struct iio_dev *iio_dev = sensor->hw->iio_devs[sensor->id]; + + memcpy(iio_buf, tmp, 3); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buf, timestamp); +} + +static irqreturn_t st_sths34pf80_handler_irq(int irq, void *private) +{ + struct st_sths34pf80_hw *hw = (struct st_sths34pf80_hw *)private; + + hw->ts = st_sths34pf80_get_time_ns(hw); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_sths34pf80_handler_thread(int irq, void *private) +{ + struct st_sths34pf80_hw *hw = (struct st_sths34pf80_hw *)private; + struct st_sths34pf80_sensor *sensor; + struct iio_dev *iio_dev; + u8 func_status; + u8 data[4]; + s64 event; + int err; + int id; + + mutex_lock(&hw->page_lock); + + err = regmap_bulk_read(hw->regmap, ST_STHS34PF80_FUNC_STATUS_ADDR, + &func_status, 1); + if (err < 0) + goto out_err; + + if (hw->enable_mask & BIT(ST_STHS34PF80_ID_TAMB_OBJ)) { + id = ST_STHS34PF80_ID_TAMB_OBJ; + sensor = iio_priv(hw->iio_devs[id]); + err = regmap_bulk_read(hw->regmap, + hw->iio_devs[id]->channels->address, + data, 4); + if (err < 0) + goto out_err; + + st_sths34pf80_report_2event(sensor, data, hw->ts); + } + +#ifdef ST_STHS34PF80_TCOMP + if (hw->enable_mask & BIT(ST_STHS34PF80_ID_TOBJECT_COMP)) { + id = ST_STHS34PF80_ID_TOBJECT_COMP; + sensor = iio_priv(hw->iio_devs[id]); + err = regmap_bulk_read(hw->regmap, + hw->iio_devs[id]->channels->address, + data, 2); + if (err < 0) + goto out_err; + + st_sths34pf80_report_event(sensor, data, hw->ts); + } +#endif /* ST_STHS34PF80_TCOMP */ + + if (hw->enable_mask & BIT(ST_STHS34PF80_ID_TAMB_SHOCK)) { + id = ST_STHS34PF80_ID_TAMB_SHOCK; + sensor = iio_priv(hw->iio_devs[id]); + err = regmap_bulk_read(hw->regmap, + hw->iio_devs[id]->channels->address, + data, 2); + if (err < 0) + goto out_err; + + data[2] = (u8)(func_status & ST_STHS34PF80_TAMB_SHOCK_FLAG_MASK) ? 1 : 0; + st_sths34pf80_report_algo_event(sensor, data, hw->ts); + } + + if (hw->enable_mask & BIT(ST_STHS34PF80_ID_TMOTION)) { + id = ST_STHS34PF80_ID_TMOTION; + sensor = iio_priv(hw->iio_devs[id]); + err = regmap_bulk_read(hw->regmap, + hw->iio_devs[id]->channels->address, + data, 2); + if (err < 0) + goto out_err; + + data[2] = (u8)(func_status & ST_STHS34PF80_MOT_FLAG_MASK) ? 1 : 0; + st_sths34pf80_report_algo_event(sensor, data, hw->ts); + } + + if (hw->enable_mask & BIT(ST_STHS34PF80_ID_TPRESENCE)) { + id = ST_STHS34PF80_ID_TPRESENCE; + sensor = iio_priv(hw->iio_devs[id]); + err = regmap_bulk_read(hw->regmap, + hw->iio_devs[id]->channels->address, + data, 2); + if (err < 0) + goto out_err; + + data[2] = (u8)(func_status & ST_STHS34PF80_PRES_FLAG_MASK) ? 1 : 0; + st_sths34pf80_report_algo_event(sensor, data, hw->ts); + } + + if (func_status & ST_STHS34PF80_PRES_FLAG_MASK && + hw->event_mask & BIT(ST_STHS34PF80_ID_TPRESENCE)) { + iio_dev = hw->iio_devs[ST_STHS34PF80_ID_TPRESENCE]; + event = IIO_UNMOD_EVENT_CODE(IIO_TEMP, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, hw->ts); + } + + if (func_status & ST_STHS34PF80_MOT_FLAG_MASK && + hw->event_mask & BIT(ST_STHS34PF80_ID_TMOTION)) { + iio_dev = hw->iio_devs[ST_STHS34PF80_ID_TMOTION]; + event = IIO_UNMOD_EVENT_CODE(IIO_TEMP, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, hw->ts); + } + + if (func_status & ST_STHS34PF80_TAMB_SHOCK_FLAG_MASK && + hw->event_mask & BIT(ST_STHS34PF80_ID_TAMB_SHOCK)) { + iio_dev = hw->iio_devs[ST_STHS34PF80_ID_TAMB_SHOCK]; + event = IIO_UNMOD_EVENT_CODE(IIO_TEMP, -1, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING); + iio_push_event(iio_dev, event, hw->ts); + } + +out_err: + mutex_unlock(&hw->page_lock); + + return IRQ_HANDLED; +} + +static int st_sths34pf80_int_config(struct st_sths34pf80_hw *hw) +{ + bool edge_trigger = false; + unsigned long irq_type; + bool irq_active_low; + int err; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); + if (irq_type == IRQF_TRIGGER_NONE) + irq_type = IRQF_TRIGGER_RISING; + + switch (irq_type) { + case IRQF_TRIGGER_RISING: + edge_trigger = true; + irq_active_low = false; + break; + case IRQF_TRIGGER_HIGH: + irq_active_low = false; + break; + case IRQF_TRIGGER_FALLING: + edge_trigger = true; + irq_active_low = true; + break; + case IRQF_TRIGGER_LOW: + irq_active_low = true; + break; + default: + dev_err(hw->dev, "mode %lx unsupported\n", irq_type); + + return -EINVAL; + } + + err = st_sths34pf80_update_bits_locked(hw, ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_H_L_MASK, + irq_active_low ? 1 : 0); + if (err < 0) + return err; + + if (device_property_read_bool(hw->dev, "drive-open-drain")) { + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_PP_OD_MASK, 1); + if (err < 0) + return err; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_sths34pf80_handler_irq, + st_sths34pf80_handler_thread, + irq_type | IRQF_ONESHOT, + "sths34pf80", hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + + return err; + } + + if (edge_trigger) { + /* set data ready pulsed */ + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_LATCHED_MASK, + 0); + if (err < 0) + return err; + + hw->edge_trigger = true; + } else { + /* set data ready latched */ + err = st_sths34pf80_update_bits_locked(hw, + ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_INT_LATCHED_MASK, + 1); + if (err < 0) + return err; + + hw->edge_trigger = false; + } + + /* enable int_or in pulsed mode */ + err = st_sths34pf80_update_cfg(hw, ST_STHS34PF80_ALGO_CONFIG_ADDR, + ST_STHS34PF80_INT_PULSED_MASK, 1); + if (err) + return err; + + err = st_sths34pf80_update_bits_locked(hw, ST_STHS34PF80_CTRL3_ADDR, + ST_STHS34PF80_IEN_MASK, 0); + + return err < 0 ? err : 0; +} + +static int st_sths34pf80_update_lpf(struct st_sths34pf80_sensor *sensor, + u8 val) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + int err, odr; + + mutex_lock(&hw->page_lock); + + /* disable odr */ + err = regmap_bulk_read(sensor->hw->regmap, + st_sths34pf80_odr_table.reg.addr, + &odr, 1); + if (err) + goto out_err; + + err = __st_sths34pf80_write_with_mask(sensor->hw, + st_sths34pf80_odr_table.reg.addr, + st_sths34pf80_odr_table.reg.mask, 0); + if (err) + goto out_err; + + err = __st_sths34pf80_write_with_mask(sensor->hw, + sensor->lpf.reg, + sensor->lpf.mask, val); + if (err) + goto out_err; + + err = st_sths34pf80_reset_algos(hw); + if (err) + goto out_err; + + /* restore odr */ + err = regmap_write(sensor->hw->regmap, + st_sths34pf80_odr_table.reg.addr, odr); + if (err < 0) + goto out_err; + + sensor->lpf.val = val; + +out_err: + mutex_unlock(&hw->page_lock); + + return 0; +} + +static ssize_t st_sths34pf80_lpf_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_sths34pf80_sensor *sensor = + iio_priv(dev_to_iio_dev(dev)); + + return sprintf(buf, "%d\n", sensor->lpf.val); +} + +static ssize_t st_sths34pf80_lpf_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_sths34pf80_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_sths34pf80_update_lpf(sensor, val); + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static int st_sths34pf80_update_threshold(struct st_sths34pf80_sensor *sensor, + u16 val) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + __le16 wdata; + int err, odr; + + mutex_lock(&hw->page_lock); + + /* disable odr */ + err = regmap_bulk_read(hw->regmap, + st_sths34pf80_odr_table.reg.addr, + &odr, 1); + if (err) + goto out_err; + + err = __st_sths34pf80_write_with_mask(hw, + st_sths34pf80_odr_table.reg.addr, + st_sths34pf80_odr_table.reg.mask, 0); + if (err) + goto out_err; + + wdata = cpu_to_le16(val); + err = st_sths34pf80_write_cfg(hw, sensor->threshold.reg, + (u8 *)&wdata, sizeof(wdata)); + if (err) + goto out_err; + + err = st_sths34pf80_reset_algos(hw); + if (err) + goto out_err; + + /* restore odr */ + err = regmap_write(sensor->hw->regmap, + st_sths34pf80_odr_table.reg.addr, odr); + if (err < 0) + goto out_err; + + sensor->threshold.val = val; + +out_err: + mutex_unlock(&hw->page_lock); + + return 0; +} + +static ssize_t st_sths34pf80_threshold_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_sths34pf80_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); + + return sprintf(buf, "%d\n", sensor->threshold.val); +} + +static ssize_t st_sths34pf80_threshold_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_sths34pf80_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_sths34pf80_update_threshold(sensor, val); + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static int st_sths34pf80_update_hysteresis(struct st_sths34pf80_sensor *sensor, + u8 val) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + int err, odr; + + mutex_lock(&hw->page_lock); + + /* disable odr */ + err = regmap_bulk_read(hw->regmap, + st_sths34pf80_odr_table.reg.addr, + &odr, 1); + if (err) + goto out_err; + + err = __st_sths34pf80_write_with_mask(hw, + st_sths34pf80_odr_table.reg.addr, + st_sths34pf80_odr_table.reg.mask, 0); + if (err) + goto out_err; + + err = st_sths34pf80_write_cfg(hw, sensor->hysteresis.reg, + &val, sizeof(val)); + if (err) + goto out_err; + + err = st_sths34pf80_reset_algos(hw); + if (err) + goto out_err; + + /* restore odr */ + err = regmap_write(sensor->hw->regmap, + st_sths34pf80_odr_table.reg.addr, odr); + if (err < 0) + goto out_err; + + sensor->hysteresis.val = val; + +out_err: + mutex_unlock(&hw->page_lock); + + return 0; +} + +static ssize_t st_sths34pf80_hysteresis_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_sths34pf80_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); + + return sprintf(buf, "%d\n", sensor->hysteresis.val); +} + +static ssize_t st_sths34pf80_hysteresis_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_sths34pf80_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_sths34pf80_update_hysteresis(sensor, val); + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_sths34pf80_sysfs_sampling_freq_avail); + +#ifdef ST_STHS34PF80_TCOMP +static ssize_t +st_sths34pf80_tobject_tcomp_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_sths34pf80_sensor *sensor = + iio_priv(dev_to_iio_dev(dev)); + + return sprintf(buf, "%d\n", sensor->hw->tcomp ? 1 : 0); +} + +static ssize_t +st_sths34pf80_tobject_tcomp_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_sths34pf80_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_sths34pf80_enable_comp(sensor->hw, val); + if (err < 0) + goto out; + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static int st_sths34pf80_read_sensitivity(struct st_sths34pf80_hw *hw) +{ + u8 sensitivity; + int err; + + err = st_sths34pf80_read_locked(hw, + ST_STHS34PF80_SENSITIVITY_DATA_ADDR, + &sensitivity, + sizeof(sensitivity)); + if (err) + return err; + + hw->sensitivity = sensitivity; + + return 0; +} + +static int st_sths34pf80_update_sensitivity(struct st_sths34pf80_sensor *sensor, + u8 val) +{ + struct st_sths34pf80_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->page_lock); + + err = regmap_write(hw->regmap, + ST_STHS34PF80_SENSITIVITY_DATA_ADDR, + val); + if (err) + goto out_err; + + hw->sensitivity = val; + +out_err: + mutex_unlock(&hw->page_lock); + + return 0; +} + +static ssize_t st_sths34pf80_sensitivity_get(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_sths34pf80_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); + int err; + + err = st_sths34pf80_read_sensitivity(sensor->hw); + if (err) + return err; + + return sprintf(buf, "%d\n", sensor->hw->sensitivity); +} + +static ssize_t st_sths34pf80_sensitivity_set(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_sths34pf80_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_sths34pf80_update_sensitivity(sensor, val); + +out: + iio_device_release_direct_mode(iio_dev); + + return err < 0 ? err : size; +} + +static IIO_DEVICE_ATTR(tcomp, 0644, + st_sths34pf80_tobject_tcomp_get, + st_sths34pf80_tobject_tcomp_set, 0); +static IIO_DEVICE_ATTR(sensitivity, 0644, + st_sths34pf80_sensitivity_get, + st_sths34pf80_sensitivity_set, 0); + +static struct attribute *st_sths34pf80_tobject_comp_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_tcomp.dev_attr.attr, + &iio_dev_attr_sensitivity.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_sths34pf80_tobject_comp_attribute_group = { + .attrs = st_sths34pf80_tobject_comp_attributes, +}; + +static const struct iio_info st_sths34pf80_tobject_comp_info = { + .attrs = &st_sths34pf80_tobject_comp_attribute_group, + .read_raw = st_sths34pf80_read_raw, + .write_raw = st_sths34pf80_write_raw, +}; +#endif /* ST_STHS34PF80_TCOMP */ + +static ssize_t st_sths34pf80_algo_reset(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_sths34pf80_sensor *sensor = iio_priv(iio_dev); + struct st_sths34pf80_hw *hw = sensor->hw; + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + mutex_lock(&hw->page_lock); + err = st_sths34pf80_reset_algos(hw); + mutex_unlock(&hw->page_lock); + iio_device_release_direct_mode(iio_dev); + + return err ? err : size; +} + +static IIO_DEVICE_ATTR(algo_reset, 0200, NULL, + st_sths34pf80_algo_reset, 0); + +static struct attribute *st_sths34pf80_tobj_amb_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_sths34pf80_tobj_amb_attribute_group = { + .attrs = st_sths34pf80_tobj_amb_attributes, +}; + +static const struct iio_info st_sths34pf80_tobj_amb_info = { + .attrs = &st_sths34pf80_tobj_amb_attribute_group, + .read_raw = st_sths34pf80_read_raw, + .write_raw = st_sths34pf80_write_raw, +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_sths34pf80_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static IIO_DEVICE_ATTR(lpf, 0644, + st_sths34pf80_lpf_get, + st_sths34pf80_lpf_set, 0); +static IIO_DEVICE_ATTR(threshold, 0644, + st_sths34pf80_threshold_get, + st_sths34pf80_threshold_set, 0); +static IIO_DEVICE_ATTR(hysteresis, 0644, + st_sths34pf80_hysteresis_get, + st_sths34pf80_hysteresis_set, 0); + +static struct attribute *st_sths34pf80_tpresence_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_lpf.dev_attr.attr, + &iio_dev_attr_threshold.dev_attr.attr, + &iio_dev_attr_hysteresis.dev_attr.attr, + &iio_dev_attr_algo_reset.dev_attr.attr, + &iio_dev_attr_sensitivity.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_sths34pf80_tpresence_attribute_group = { + .attrs = st_sths34pf80_tpresence_attributes, +}; + +static const struct iio_info st_sths34pf80_tpresence_info = { + .attrs = &st_sths34pf80_tpresence_attribute_group, + .read_raw = st_sths34pf80_read_raw, + .write_raw = st_sths34pf80_write_raw, + .read_event_config = st_sths34pf80_read_event_config, + .write_event_config = st_sths34pf80_write_event_config, +#ifdef CONFIG_DEBUG_FS + .debugfs_reg_access = &st_sths34pf80_reg_access, +#endif /* CONFIG_DEBUG_FS */ +}; + +static struct attribute *st_sths34pf80_tmotion_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_lpf.dev_attr.attr, + &iio_dev_attr_threshold.dev_attr.attr, + &iio_dev_attr_hysteresis.dev_attr.attr, + &iio_dev_attr_algo_reset.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_sths34pf80_tmotion_attribute_group = { + .attrs = st_sths34pf80_tmotion_attributes, +}; + +static const struct iio_info st_sths34pf80_tmotion_info = { + .attrs = &st_sths34pf80_tmotion_attribute_group, + .read_raw = st_sths34pf80_read_raw, + .write_raw = st_sths34pf80_write_raw, + .read_event_config = st_sths34pf80_read_event_config, + .write_event_config = st_sths34pf80_write_event_config, +}; + +static struct attribute *st_sths34pf80_tambshock_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_lpf.dev_attr.attr, + &iio_dev_attr_threshold.dev_attr.attr, + &iio_dev_attr_algo_reset.dev_attr.attr, + NULL, +}; + +static const struct +attribute_group st_sths34pf80_tambshock_attribute_group = { + .attrs = st_sths34pf80_tambshock_attributes, +}; + +static const struct iio_info st_sths34pf80_tambshock_info = { + .attrs = &st_sths34pf80_tambshock_attribute_group, + .read_raw = st_sths34pf80_read_raw, + .write_raw = st_sths34pf80_write_raw, + .read_event_config = st_sths34pf80_read_event_config, + .write_event_config = st_sths34pf80_write_event_config, +}; + +static const unsigned long st_sths34pf80_tobject_com_available_scan_masks[] = { + BIT(1), BIT(0) +}; + +static const unsigned long st_sths34pf80_tobj_amb_available_scan_masks[] = { + GENMASK(1, 0), BIT(0) +}; + +static const unsigned long st_sths34pf80_available_algo_scan_masks[] = { + GENMASK(1, 0), BIT(0) +}; + +static int st_sths34pf80_init_device(struct st_sths34pf80_hw *hw) +{ + int err; + + /* enable Block Data Update */ + err = regmap_update_bits(hw->regmap, ST_STHS34PF80_CTRL1_ADDR, + ST_STHS34PF80_BDU_MASK, + FIELD_PREP(ST_STHS34PF80_BDU_MASK, 1)); + if (err < 0) + return err; + +#ifdef ST_STHS34PF80_TCOMP + err = st_sths34pf80_enable_comp(hw, true); + if (err < 0) + return err; +#endif /* ST_STHS34PF80_TCOMP */ + + err = st_sths34pf80_int_config(hw); + if (err < 0) { + dev_info(hw->dev, "unable to configure interrupt line (%d)\n", + err); + + return err; + } + + return 0; +} + +static struct iio_dev * +st_sths34pf80_alloc_iiodev(struct st_sths34pf80_hw *hw, + enum st_sths34pf80_sensor_id id) +{ + struct st_sths34pf80_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; + + switch (id) { + case ST_STHS34PF80_ID_TAMB_OBJ: + iio_dev->channels = st_sths34pf80_tobj_amb_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_sths34pf80_tobj_amb_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "sths34pf80_tobjamb"); + iio_dev->info = &st_sths34pf80_tobj_amb_info; + iio_dev->available_scan_masks = + st_sths34pf80_tobj_amb_available_scan_masks; + sensor->odr = st_sths34pf80_odr_table.odr_avl[1].hz; + sensor->uodr = st_sths34pf80_odr_table.odr_avl[1].uhz; + break; +#ifdef ST_STHS34PF80_TCOMP + case ST_STHS34PF80_ID_TOBJECT_COMP: + iio_dev->channels = st_sths34pf80_tobject_comp_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_sths34pf80_tobject_comp_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "sths34pf80_tobject_comp"); + iio_dev->info = &st_sths34pf80_tobject_comp_info; + iio_dev->available_scan_masks = + st_sths34pf80_tobject_com_available_scan_masks; + sensor->odr = st_sths34pf80_odr_table.odr_avl[1].hz; + sensor->uodr = + st_sths34pf80_odr_table.odr_avl[1].uhz; + break; +#endif /* ST_STHS34PF80_TCOMP */ + case ST_STHS34PF80_ID_TAMB_SHOCK: + iio_dev->channels = st_sths34pf80_tambshock_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_sths34pf80_tambshock_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "sths34pf80_tambshock"); + iio_dev->info = &st_sths34pf80_tambshock_info; + iio_dev->available_scan_masks = + st_sths34pf80_available_algo_scan_masks; + sensor->odr = st_sths34pf80_odr_table.odr_avl[1].hz; + sensor->uodr = st_sths34pf80_odr_table.odr_avl[1].uhz; + sensor->lpf.reg = ST_STHS34PF80_LPF2_ADDR; + sensor->lpf.mask = ST_STHS34PF80_LPF_A_T_MASK; + st_sths34pf80_update_lpf(sensor, + ST_STHS34PF80_LPF_A_T_DEFAULT); + sensor->threshold.reg = ST_STHS34PF80_TAMB_SHOCK_THS_ADDR; + st_sths34pf80_update_threshold(sensor, + ST_STHS34PF80_TAMB_SHOCK_THS_DEFAULT); + break; + case ST_STHS34PF80_ID_TMOTION: + iio_dev->channels = st_sths34pf80_tmotion_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_sths34pf80_tmotion_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "sths34pf80_tmotion"); + iio_dev->info = &st_sths34pf80_tmotion_info; + iio_dev->available_scan_masks = + st_sths34pf80_available_algo_scan_masks; + sensor->odr = st_sths34pf80_odr_table.odr_avl[1].hz; + sensor->uodr = st_sths34pf80_odr_table.odr_avl[1].uhz; + sensor->lpf.reg = ST_STHS34PF80_LPF1_ADDR; + sensor->lpf.mask = ST_STHS34PF80_LPF_M_MASK; + st_sths34pf80_update_lpf(sensor, + ST_STHS34PF80_LPF_M_DEFAULT); + sensor->threshold.reg = ST_STHS34PF80_MOTION_THS_ADDR; + st_sths34pf80_update_threshold(sensor, + ST_STHS34PF80_MOTION_THS_DEFAULT); + sensor->hysteresis.reg = ST_STHS34PF80_HYST_MOTION_ADDR; + st_sths34pf80_update_hysteresis(sensor, + ST_STHS34PF80_HYST_MOTION_DEFAULT); + break; + case ST_STHS34PF80_ID_TPRESENCE: + iio_dev->channels = st_sths34pf80_tpresence_channels; + iio_dev->num_channels = + ARRAY_SIZE(st_sths34pf80_tpresence_channels); + scnprintf(sensor->name, sizeof(sensor->name), + "sths34pf80_tpresence"); + iio_dev->info = &st_sths34pf80_tpresence_info; + iio_dev->available_scan_masks = + st_sths34pf80_available_algo_scan_masks; + sensor->odr = st_sths34pf80_odr_table.odr_avl[1].hz; + sensor->uodr = st_sths34pf80_odr_table.odr_avl[1].uhz; + sensor->lpf.reg = ST_STHS34PF80_LPF2_ADDR; + sensor->lpf.mask = ST_STHS34PF80_LPF_P_MASK; + st_sths34pf80_update_lpf(sensor, + ST_STHS34PF80_LPF_P_DEFAULT); + sensor->threshold.reg = ST_STHS34PF80_PRESENCE_THS_ADDR; + st_sths34pf80_update_threshold(sensor, + ST_STHS34PF80_PRESENCE_THS_DEFAULT); + sensor->hysteresis.reg = ST_STHS34PF80_HYST_PRESENCE_ADDR; + st_sths34pf80_update_hysteresis(sensor, + ST_STHS34PF80_HYST_PRESENCE_DEFAULT); + break; + default: + return NULL; + } + + iio_dev->name = sensor->name; + + return iio_dev; +} + +static void st_sths34pf80_disable_regulator_action(void *_data) +{ + struct st_sths34pf80_hw *hw = _data; + + regulator_disable(hw->vddio_supply); + regulator_disable(hw->vdd_supply); +} + +static int st_sths34pf80_power_enable(struct st_sths34pf80_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_sths34pf80_disable_regulator_action, hw); + if (err) { + dev_err(hw->dev, + "Failed to setup regulator cleanup action %d\n", + err); + + return err; + } + + return 0; +} + +static int st_sths34pf80_preenable(struct iio_dev *iio_dev) +{ + struct st_sths34pf80_sensor *sensor = iio_priv(iio_dev); + + return st_sths34pf80_sensor_set_enable(sensor, true); +} + +static int st_sths34pf80_postdisable(struct iio_dev *iio_dev) +{ + struct st_sths34pf80_sensor *sensor = iio_priv(iio_dev); + + return st_sths34pf80_sensor_set_enable(sensor, false); +} + +static const struct iio_buffer_setup_ops st_sths34pf80_fifo_ops = { + .preenable = st_sths34pf80_preenable, + .postdisable = st_sths34pf80_postdisable, +}; + +/** + * Probe device function + * + * @param dev: Device pointer. + * @param irq: I2C/SPI/I3C client irq. + * @param regmap: Bus Transfer Function pointer. + * @retval 0 if OK, < 0 for error + */ +int st_sths34pf80_probe(struct device *dev, int irq, + struct regmap *regmap) +{ + struct st_sths34pf80_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->page_lock); + mutex_init(&hw->int_lock); + + hw->regmap = regmap; + hw->dev = dev; + hw->irq = irq; + + err = st_sths34pf80_power_enable(hw); + if (err != 0) + return err; + + /* wait sths34pf80 power up after enabling regulator */ + usleep_range(2500, 2600); + + err = st_sths34pf80_check_whoami(hw); + if (err < 0) + return err; + + err = st_sths34pf80_init_device(hw); + if (err < 0) + return err; + + for (i = 0; i < ST_STHS34PF80_ID_MAX; i++) { + +#if KERNEL_VERSION(5, 13, 0) > LINUX_VERSION_CODE + struct iio_buffer *buffer; +#endif /* LINUX_VERSION_CODE */ + + hw->iio_devs[i] = st_sths34pf80_alloc_iiodev(hw, i); + if (!hw->iio_devs[i]) + return -ENOMEM; + +#if KERNEL_VERSION(5, 19, 0) <= LINUX_VERSION_CODE + err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + &st_sths34pf80_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[i], + INDIO_BUFFER_SOFTWARE, + &st_sths34pf80_fifo_ops); + if (err) + return err; +#else /* LINUX_VERSION_CODE */ + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_sths34pf80_fifo_ops; +#endif /* LINUX_VERSION_CODE */ + + err = devm_iio_device_register(hw->dev, + hw->iio_devs[i]); + if (err) + return err; + } + + dev_info(dev, "device probed\n"); + + return 0; +} +EXPORT_SYMBOL(st_sths34pf80_probe); + +int st_sths34pf80_remove(struct device *dev) +{ + struct st_sths34pf80_hw *hw = dev_get_drvdata(dev); + struct st_sths34pf80_sensor *sensor; + int i; + + for (i = 0; i < ST_STHS34PF80_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + st_sths34pf80_sensor_set_enable(sensor, false); + st_sths34pf80_event_sensor_enable(sensor, false); + } + + return 0; +} +EXPORT_SYMBOL(st_sths34pf80_remove); + +static int __maybe_unused st_sths34pf80_suspend(struct device *dev) +{ + struct st_sths34pf80_hw *hw = dev_get_drvdata(dev); + struct st_sths34pf80_sensor *sensor; + int i; + + for (i = 0; i < ST_STHS34PF80_ID_MAX; i++) { + int err; + + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_sths34pf80_set_odr(sensor, 0, 0); + if (err < 0) + return err; + } + + return 0; +} + +static int __maybe_unused st_sths34pf80_resume(struct device *dev) +{ + struct st_sths34pf80_hw *hw = dev_get_drvdata(dev); + struct st_sths34pf80_sensor *sensor; + int i; + + for (i = 0; i < ST_STHS34PF80_ID_MAX; i++) { + int err; + + sensor = iio_priv(hw->iio_devs[i]); + if (!hw->iio_devs[i]) + continue; + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_sths34pf80_set_odr(sensor, sensor->odr, + sensor->uodr); + if (err < 0) + return err; + } + + return 0; +} + +#ifdef CONFIG_PM_SLEEP +const struct dev_pm_ops st_sths34pf80_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_sths34pf80_suspend, + st_sths34pf80_resume) +}; +EXPORT_SYMBOL(st_sths34pf80_pm_ops); +#endif /* CONFIG_PM_SLEEP */ + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_sths34pf80 driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_i2c.c b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_i2c.c new file mode 100644 index 000000000000..c4dc3c29ac92 --- /dev/null +++ b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_i2c.c @@ -0,0 +1,82 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_sths34pf80 i2c driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_sths34pf80.h" + +static const struct regmap_config st_sths34pf80_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_sths34pf80_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, + &st_sths34pf80_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_sths34pf80_probe(&client->dev, client->irq, regmap); +} + +#if KERNEL_VERSION(6, 1, 0) <= LINUX_VERSION_CODE +static void st_sths34pf80_i2c_remove(struct i2c_client *client) +{ + st_sths34pf80_remove(&client->dev); +} +#else /* LINUX_VERSION_CODE */ +static int st_sths34pf80_i2c_remove(struct i2c_client *client) +{ + return st_sths34pf80_remove(&client->dev); +} +#endif /* LINUX_VERSION_CODE */ + +static const struct of_device_id st_sths34pf80_i2c_of_match[] = { + { .compatible = "st," ST_STHS34PF80_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_sths34pf80_i2c_of_match); + +static const struct i2c_device_id st_sths34pf80_i2c_id_table[] = { + { ST_STHS34PF80_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_sths34pf80_i2c_id_table); + +static struct i2c_driver st_sths34pf80_driver = { + .driver = { + .name = "st_" ST_STHS34PF80_DEV_NAME "_i2c", +#ifdef CONFIG_PM_SLEEP + .pm = &st_sths34pf80_pm_ops, +#endif /* CONFIG_PM_SLEEP */ + .of_match_table = + of_match_ptr(st_sths34pf80_i2c_of_match), + }, + .probe = st_sths34pf80_i2c_probe, + .remove = st_sths34pf80_i2c_remove, + .id_table = st_sths34pf80_i2c_id_table, +}; +module_i2c_driver(st_sths34pf80_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_sths34pf80 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_spi.c b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_spi.c new file mode 100644 index 000000000000..9e435bc801f1 --- /dev/null +++ b/drivers/iio/stm/tmos/st_sths34pf80/st_sths34pf80_spi.c @@ -0,0 +1,81 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_sths34pf80 spi driver + * + * MEMS Software Solutions Team + * + * Copyright 2022 STMicroelectronics Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "st_sths34pf80.h" + +static const struct regmap_config st_sths34pf80_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_sths34pf80_spi_probe(struct spi_device *spi) +{ + struct regmap *regmap; + + regmap = devm_regmap_init_spi(spi, + &st_sths34pf80_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_sths34pf80_probe(&spi->dev, spi->irq, regmap); +} + +#if KERNEL_VERSION(5, 18, 0) <= LINUX_VERSION_CODE +static void st_sths34pf80_i2c_remove(struct spi_device *spi) +{ + st_sths34pf80_remove(&spi->dev); +} +#else /* LINUX_VERSION_CODE */ +static int st_sths34pf80_i2c_remove(struct spi_device *spi) +{ + return st_sths34pf80_remove(&spi->dev); +} +#endif /* LINUX_VERSION_CODE */ + +static const struct of_device_id st_sths34pf80_spi_of_match[] = { + { .compatible = "st," ST_STHS34PF80_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_sths34pf80_spi_of_match); + +static const struct spi_device_id st_sths34pf80_spi_id_table[] = { + { ST_STHS34PF80_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_sths34pf80_spi_id_table); + +static struct spi_driver st_sths34pf80_driver = { + .driver = { + .name = "st_" ST_STHS34PF80_DEV_NAME "_spi", +#ifdef CONFIG_PM_SLEEP + .pm = &st_sths34pf80_pm_ops, +#endif /* CONFIG_PM_SLEEP */ + .of_match_table = + of_match_ptr(st_sths34pf80_spi_of_match), + }, + .probe = st_sths34pf80_spi_probe, + .remove = st_sths34pf80_i2c_remove, + .id_table = st_sths34pf80_spi_id_table, +}; +module_spi_driver(st_sths34pf80_driver); + +MODULE_AUTHOR("MEMS Software Solutions Team"); +MODULE_DESCRIPTION("STMicroelectronics st_sths34pf80 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/include/linux/platform_data/stm/ism303dac.h b/include/linux/platform_data/stm/ism303dac.h new file mode 100644 index 000000000000..1b26adb4d07c --- /dev/null +++ b/include/linux/platform_data/stm/ism303dac.h @@ -0,0 +1,17 @@ +/* + * STMicroelectronics ism303dac driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Licensed under the GPL-2. + */ + + +#ifndef __ISM303DAC_H__ +#define __ISM303DAC_H__ + +struct ism303dac_platform_data { + u8 drdy_int_pin; +}; + +#endif /* __ISM303DAC_H__ */ diff --git a/include/linux/platform_data/stm/lis2ds12.h b/include/linux/platform_data/stm/lis2ds12.h new file mode 100644 index 000000000000..fec9fccbc0f9 --- /dev/null +++ b/include/linux/platform_data/stm/lis2ds12.h @@ -0,0 +1,22 @@ +/* + * STMicroelectronics lis2ds12 driver + * + * Copyright 2015 STMicroelectronics Inc. + * + * Giuseppe Barba + * + * Licensed under the GPL-2. + */ + + +#ifndef __LIS2DS12_H__ +#define __LIS2DS12_H__ + +#define LIS2DS12_DEV_NAME "lis2ds12" +#define LIS2DS12_I2C_ADDR 0x1e + +struct lis2ds12_platform_data { + u8 drdy_int_pin; +}; + +#endif /* __LIS2DS12_H__ */ diff --git a/include/linux/platform_data/stm/lis2hh12.h b/include/linux/platform_data/stm/lis2hh12.h new file mode 100644 index 000000000000..347d7e5b9d96 --- /dev/null +++ b/include/linux/platform_data/stm/lis2hh12.h @@ -0,0 +1,17 @@ +/* + * STMicroelectronics lis2hh12 driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Licensed under the GPL-2. + */ + + +#ifndef __LIS2HH12_H__ +#define __LIS2HH12_H__ + +struct lis2hh12_platform_data { + u8 drdy_int_pin; +}; + +#endif /* __LIS2HH12_H__ */ diff --git a/stm_iio_configs/acc33_defconfig b/stm_iio_configs/acc33_defconfig new file mode 100644 index 000000000000..ac791e02f60c --- /dev/null +++ b/stm_iio_configs/acc33_defconfig @@ -0,0 +1,3 @@ +CONFIG_IIO_ST_ACC33=m +CONFIG_IIO_ST_ACC33_I2C=m +CONFIG_IIO_ST_ACC33_SPI=m diff --git a/stm_iio_configs/asm330lhhx_defconfig b/stm_iio_configs/asm330lhhx_defconfig new file mode 100644 index 000000000000..2493b1f9533b --- /dev/null +++ b/stm_iio_configs/asm330lhhx_defconfig @@ -0,0 +1,6 @@ +CONFIG_IIO_ST_ASM330LHHX=m +CONFIG_IIO_ST_ASM330LHHX_I2C=m +CONFIG_IIO_ST_ASM330LHHX_SPI=m +CONFIG_IIO_ST_ASM330LHHX_EN_BASIC_FEATURES=y +CONFIG_IIO_ST_ASM330LHHX_MAY_WAKEUP=y +CONFIG_IIO_ST_ASM330LHHX_MLC_PRELOAD=y diff --git a/stm_iio_configs/common_defconfig b/stm_iio_configs/common_defconfig new file mode 100644 index 000000000000..81c31a0dc04d --- /dev/null +++ b/stm_iio_configs/common_defconfig @@ -0,0 +1 @@ +CONFIG_IIO_STM=y diff --git a/stm_iio_configs/imu68_defconfig b/stm_iio_configs/imu68_defconfig new file mode 100644 index 000000000000..3a94850bc140 --- /dev/null +++ b/stm_iio_configs/imu68_defconfig @@ -0,0 +1,3 @@ +CONFIG_IIO_ST_IMU68=m +CONFIG_IIO_ST_IMU68_I2C=m +CONFIG_IIO_ST_IMU68_SPI=m diff --git a/stm_iio_configs/ism303dac_defconfig b/stm_iio_configs/ism303dac_defconfig new file mode 100644 index 000000000000..6b9c23f3a18f --- /dev/null +++ b/stm_iio_configs/ism303dac_defconfig @@ -0,0 +1,4 @@ +CONFIG_IIO_ST_ISM303DAC_ACCEL=m +CONFIG_IIO_ST_ISM303DAC_ACCEL_I2C=m +CONFIG_IIO_ST_ISM303DAC_ACCEL_SPI=m +CONFIG_ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO=0 diff --git a/stm_iio_configs/ism330dhcx_defconfig b/stm_iio_configs/ism330dhcx_defconfig new file mode 100644 index 000000000000..7ea371302359 --- /dev/null +++ b/stm_iio_configs/ism330dhcx_defconfig @@ -0,0 +1,4 @@ +CONFIG_IIO_ST_ISM330DHCX=m +CONFIG_IIO_ST_ISM330DHCX_I2C=m +CONFIG_IIO_ST_ISM330DHCX_SPI=m +CONFIG_IIO_ST_ISM330DHCX_MAY_WAKEUP=y diff --git a/stm_iio_configs/ism330dlc_defconfig b/stm_iio_configs/ism330dlc_defconfig new file mode 100644 index 000000000000..09b74915b7bf --- /dev/null +++ b/stm_iio_configs/ism330dlc_defconfig @@ -0,0 +1,8 @@ +CONFIG_ST_ISM330DLC_IIO=m +CONFIG_ST_ISM330DLC_I2C_IIO=m +CONFIG_ST_ISM330DLC_SPI_IIO=m +CONFIG_ST_ISM330DLC_IIO_LIMIT_FIFO=0 +CONFIG_ST_ISM330DLC_IIO_MASTER_SUPPORT=y +CONFIG_ST_ISM330DLC_ENABLE_INTERNAL_PULLUP=y +CONFIG_ST_ISM330DLC_IIO_EXT0_LIS2MDL=y +CONFIG_ST_ISM330DLC_XL_DATA_INJECTION=y diff --git a/stm_iio_configs/lis2duxs12_defconfig b/stm_iio_configs/lis2duxs12_defconfig new file mode 100644 index 000000000000..a5b3de2e38a9 --- /dev/null +++ b/stm_iio_configs/lis2duxs12_defconfig @@ -0,0 +1,5 @@ +CONFIG_IIO_ST_LIS2DUXS12=m +CONFIG_IIO_ST_LIS2DUXS12_I2C=m +CONFIG_IIO_ST_LIS2DUXS12_SPI=m +CONFIG_IIO_ST_LIS2DUXS12_I3C=m +CONFIG_IIO_ST_LIS2DUXS12_QVAR=y diff --git a/stm_iio_configs/lis2dw12_defconfig b/stm_iio_configs/lis2dw12_defconfig new file mode 100644 index 000000000000..a851f2e76e21 --- /dev/null +++ b/stm_iio_configs/lis2dw12_defconfig @@ -0,0 +1,3 @@ +CONFIG_IIO_ST_LIS2DW12=m +CONFIG_IIO_ST_LIS2DW12_I2C=m +CONFIG_IIO_ST_LIS2DW12_SPI=m diff --git a/stm_iio_configs/lis2hh12_defconfig b/stm_iio_configs/lis2hh12_defconfig new file mode 100644 index 000000000000..23a471d5cfc6 --- /dev/null +++ b/stm_iio_configs/lis2hh12_defconfig @@ -0,0 +1,3 @@ +CONFIG_IIO_ST_LIS2HH12=m +CONFIG_IIO_ST_LIS2HH12_I2C=m +CONFIG_IIO_ST_LIS2HH12_SPI=m diff --git a/stm_iio_configs/lis3dhh_defconfig b/stm_iio_configs/lis3dhh_defconfig new file mode 100644 index 000000000000..d75e554a1832 --- /dev/null +++ b/stm_iio_configs/lis3dhh_defconfig @@ -0,0 +1 @@ +CONFIG_IIO_ST_LIS3DHH=m diff --git a/stm_iio_configs/lisds12_defconfig b/stm_iio_configs/lisds12_defconfig new file mode 100644 index 000000000000..3ac8ebf06d39 --- /dev/null +++ b/stm_iio_configs/lisds12_defconfig @@ -0,0 +1,4 @@ +CONFIG_IIO_ST_LIS2DS12=m +CONFIG_IIO_ST_LIS2DS12_I2C=m +CONFIG_IIO_ST_LIS2DS12_SPI=m +CONFIG_ST_LIS2DS12_IIO_LIMIT_FIFO=0 diff --git a/stm_iio_configs/lps22df_defconfig b/stm_iio_configs/lps22df_defconfig new file mode 100644 index 000000000000..757916e675f6 --- /dev/null +++ b/stm_iio_configs/lps22df_defconfig @@ -0,0 +1,3 @@ +CONFIG_ST_LPS22DF_IIO=m +CONFIG_ST_LPS22DF_I2C_IIO=m +CONFIG_ST_LPS22DF_SPI_IIO=m diff --git a/stm_iio_configs/lps22hb_defconfig b/stm_iio_configs/lps22hb_defconfig new file mode 100644 index 000000000000..42770f42d2da --- /dev/null +++ b/stm_iio_configs/lps22hb_defconfig @@ -0,0 +1,3 @@ +CONFIG_ST_LPS22HB_IIO=m +CONFIG_ST_LPS22HB_I2C_IIO=m +CONFIG_ST_LPS22HB_SPI_IIO=m diff --git a/stm_iio_configs/lps22hh_defconfig b/stm_iio_configs/lps22hh_defconfig new file mode 100644 index 000000000000..0f3da4130cc9 --- /dev/null +++ b/stm_iio_configs/lps22hh_defconfig @@ -0,0 +1,3 @@ +CONFIG_ST_LPS22HH_IIO=m +CONFIG_ST_LPS22HH_I2C_IIO=m +CONFIG_ST_LPS22HH_SPI_IIO=m diff --git a/stm_iio_configs/lps33hw_defconfig b/stm_iio_configs/lps33hw_defconfig new file mode 100644 index 000000000000..657cb64e40b4 --- /dev/null +++ b/stm_iio_configs/lps33hw_defconfig @@ -0,0 +1,3 @@ +CONFIG_ST_LPS33HW_IIO=m +CONFIG_ST_LPS33HW_I2C_IIO=m +CONFIG_ST_LPS33HW_SPI_IIO=m diff --git a/stm_iio_configs/lsm6ds3_defconfig b/stm_iio_configs/lsm6ds3_defconfig new file mode 100644 index 000000000000..837c3c1ab3fd --- /dev/null +++ b/stm_iio_configs/lsm6ds3_defconfig @@ -0,0 +1,9 @@ +CONFIG_ST_LSM6DS3_IIO=m +CONFIG_ST_LSM6DS3_I2C_IIO=m +CONFIG_ST_LSM6DS3_SPI_IIO=m +CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO=0 +CONFIG_ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND=n +CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT=y +CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP=y +CONFIG_ST_LSM6DS3_IIO_EXT0_LIS2MDL=y +CONFIG_ST_LSM6DS3_XL_DATA_INJECTION=y diff --git a/stm_iio_configs/lsm6ds3h_defconfig b/stm_iio_configs/lsm6ds3h_defconfig new file mode 100644 index 000000000000..c0a0654caec0 --- /dev/null +++ b/stm_iio_configs/lsm6ds3h_defconfig @@ -0,0 +1,9 @@ +CONFIG_ST_LSM6DS3H_IIO=m +CONFIG_ST_LSM6DS3H_I2C_IIO=m +CONFIG_ST_LSM6DS3H_SPI_IIO=m +CONFIG_ST_LSM6DS3H_IIO_LIMIT_FIFO=0 +CONFIG_ST_LSM6DS3H_STEP_COUNTER_ON_DURING_SUSPEND=n +CONFIG_ST_LSM6DS3H_IIO_MASTER_SUPPORT=y +CONFIG_ST_LSM6DS3H_ENABLE_INTERNAL_PULLUP=y +CONFIG_ST_LSM6DS3H_IIO_EXT0_LIS2MDL=y +CONFIG_ST_LSM6DS3H_XL_DATA_INJECTION=y diff --git a/stm_iio_configs/lsm6dsm_defconfig b/stm_iio_configs/lsm6dsm_defconfig new file mode 100644 index 000000000000..cff4a91b1836 --- /dev/null +++ b/stm_iio_configs/lsm6dsm_defconfig @@ -0,0 +1,9 @@ +CONFIG_ST_LSM6DSM_IIO=m +CONFIG_ST_LSM6DSM_I2C_IIO=m +CONFIG_ST_LSM6DSM_SPI_IIO=m +CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO=0 +CONFIG_ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND=n +CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT=y +CONFIG_ST_LSM6DSM_ENABLE_INTERNAL_PULLUP=y +CONFIG_ST_LSM6DSM_IIO_EXT0_LIS2MDL=y +CONFIG_ST_LSM6DSM_XL_DATA_INJECTION=y diff --git a/stm_iio_configs/lsm6dso16is_defconfig b/stm_iio_configs/lsm6dso16is_defconfig new file mode 100644 index 000000000000..3c35409a67cf --- /dev/null +++ b/stm_iio_configs/lsm6dso16is_defconfig @@ -0,0 +1,3 @@ +CONFIG_IIO_ST_LSM6DSOX=m +CONFIG_IIO_ST_LSM6DSOX_I2C=m +CONFIG_IIO_ST_LSM6DSOX_SPI=m diff --git a/stm_iio_configs/lsm6dsox_defconfig b/stm_iio_configs/lsm6dsox_defconfig new file mode 100644 index 000000000000..61a761452387 --- /dev/null +++ b/stm_iio_configs/lsm6dsox_defconfig @@ -0,0 +1,5 @@ +CONFIG_IIO_ST_LSM6DSOX=m +CONFIG_IIO_ST_LSM6DSOX_I2C=m +CONFIG_IIO_ST_LSM6DSOX_SPI=m +CONFIG_IIO_ST_LSM6DSOX_I3C=m +CONFIG_IIO_ST_LSM6DSOX_MAY_WAKEUP=y diff --git a/stm_iio_configs/lsm6dsrx_defconfig b/stm_iio_configs/lsm6dsrx_defconfig new file mode 100644 index 000000000000..a7de27872f1e --- /dev/null +++ b/stm_iio_configs/lsm6dsrx_defconfig @@ -0,0 +1,6 @@ +CONFIG_IIO_ST_LSM6DSRX=m +CONFIG_IIO_ST_LSM6DSRX_I2C=m +CONFIG_IIO_ST_LSM6DSRX_SPI=m +CONFIG_IIO_ST_LSM6DSRX_I3C=m +CONFIG_IIO_ST_LSM6DSRX_MLC_PRELOAD=y +CONFIG_IIO_ST_LSM6DSRX_MAY_WAKEUP=y diff --git a/stm_iio_configs/lsm6dsvx_defconfig b/stm_iio_configs/lsm6dsvx_defconfig new file mode 100644 index 000000000000..69ba42977286 --- /dev/null +++ b/stm_iio_configs/lsm6dsvx_defconfig @@ -0,0 +1,6 @@ +CONFIG_IIO_ST_LSM6DSVX=m +CONFIG_IIO_ST_LSM6DSVX_I2C=m +CONFIG_IIO_ST_LSM6DSVX_SPI=m +CONFIG_IIO_ST_LSM6DSVX_MAY_WAKEUP=y +CONFIG_IIO_ST_LSM6DSVX_QVAR_IN_FIFO=y +CONFIG_IIO_ST_LSM6DSVX_MLC_PRELOAD=y diff --git a/stm_iio_configs/mag3d_defconfig b/stm_iio_configs/mag3d_defconfig new file mode 100644 index 000000000000..145d537d0b46 --- /dev/null +++ b/stm_iio_configs/mag3d_defconfig @@ -0,0 +1,3 @@ +CONFIG_ST_MAG3D_IIO=m +CONFIG_ST_MAG3D_I2C_IIO=m +CONFIG_ST_MAG3D_SPI_IIO=m diff --git a/stm_iio_configs/mag40_defconfig b/stm_iio_configs/mag40_defconfig new file mode 100644 index 000000000000..f1faf9bf562d --- /dev/null +++ b/stm_iio_configs/mag40_defconfig @@ -0,0 +1,3 @@ +CONFIG_ST_MAG40_IIO=m +CONFIG_ST_MAG40_I2C_IIO=m +CONFIG_ST_MAG40_SPI_IIO=m diff --git a/stm_iio_configs/sths34pf80_defconfig b/stm_iio_configs/sths34pf80_defconfig new file mode 100644 index 000000000000..1b397e1cb5f4 --- /dev/null +++ b/stm_iio_configs/sths34pf80_defconfig @@ -0,0 +1,3 @@ +CONFIG_IIO_ST_STHS34PF80=m +CONFIG_IIO_ST_STHS34PF80_SPI=m +CONFIG_IIO_ST_STHS34PF80_I2C=m diff --git a/stm_iio_configs/stts22h_defconfig b/stm_iio_configs/stts22h_defconfig new file mode 100644 index 000000000000..0880733be74d --- /dev/null +++ b/stm_iio_configs/stts22h_defconfig @@ -0,0 +1 @@ +CONFIG_IIO_STTS22H=m diff --git a/stm_iio_patches/4.14.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch b/stm_iio_patches/4.14.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch new file mode 100644 index 000000000000..7e43f4aacfdc --- /dev/null +++ b/stm_iio_patches/4.14.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch @@ -0,0 +1,49 @@ +From fdef8f9eff9c491f805edc1f477923830c244769 Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 16 Sep 2021 14:44:46 +0200 +Subject: [PATCH 2/2] STMEMS Added iio type patch for STMEMS + +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 11 +++++++++++ + 1 file changed, 11 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index 4213cdf..1a9affd 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -43,6 +43,14 @@ enum iio_chan_type { + IIO_ELECTRICALCONDUCTIVITY, + IIO_COUNT, + IIO_INDEX, ++ IIO_SIGN_MOTION, ++ IIO_STEP_DETECTOR, ++ IIO_STEP_COUNTER, ++ IIO_TILT, ++ IIO_TAP, ++ IIO_TAP_TAP, ++ IIO_WRIST_TILT_GESTURE, ++ IIO_GESTURE, + IIO_GRAVITY, + }; + +@@ -93,6 +101,7 @@ enum iio_event_type { + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, ++ IIO_EV_TYPE_FIFO_FLUSH, + }; + + enum iio_event_direction { +@@ -100,6 +109,8 @@ enum iio_event_direction { + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, + IIO_EV_DIR_NONE, ++ IIO_EV_DIR_FIFO_EMPTY, ++ IIO_EV_DIR_FIFO_DATA, + }; + + #endif /* _UAPI_IIO_TYPES_H_ */ +-- +2.7.4 + diff --git a/stm_iio_patches/4.14.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch b/stm_iio_patches/4.14.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch new file mode 100644 index 000000000000..42f2229c2047 --- /dev/null +++ b/stm_iio_patches/4.14.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch @@ -0,0 +1,34 @@ +From 048fe8d4c7069bf74d9382964373e44cafc6e344 Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 16 Sep 2021 14:42:13 +0200 +Subject: [PATCH 1/2] Added STMEMS sensor to build system + +Signed-off-by: mario tesi +--- + drivers/iio/Kconfig | 1 + + drivers/iio/Makefile | 1 + + 2 files changed, 2 insertions(+) + +diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig +index b3c8c6e..f34f97a 100644 +--- a/drivers/iio/Kconfig ++++ b/drivers/iio/Kconfig +@@ -93,5 +93,6 @@ source "drivers/iio/potentiostat/Kconfig" + source "drivers/iio/pressure/Kconfig" + source "drivers/iio/proximity/Kconfig" + source "drivers/iio/temperature/Kconfig" ++source "drivers/iio/stm/Kconfig" + + endif # IIO +diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile +index b16b2e9..b8596a9 100644 +--- a/drivers/iio/Makefile ++++ b/drivers/iio/Makefile +@@ -37,3 +37,4 @@ obj-y += pressure/ + obj-y += proximity/ + obj-y += temperature/ + obj-y += trigger/ ++obj-y += stm/ +-- +2.7.4 + diff --git a/stm_iio_patches/4.14.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch b/stm_iio_patches/4.14.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch new file mode 100644 index 000000000000..42ef0a555e5a --- /dev/null +++ b/stm_iio_patches/4.14.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch @@ -0,0 +1,26 @@ +From 7f632aaebf1e9558d06dafcd56cd887e34f4c5a6 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Mon, 29 Nov 2021 15:15:48 -0800 +Subject: [PATCH 1/1] stm: add new iio event used for timesync logic + +Signed-off-by: Denis Ciocca +Change-Id: I05ec93ccdb9f4905d6a6c1900386332edf21fea2 +--- + include/uapi/linux/iio/types.h | 1 + + 1 file changed, 1 insertion(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index b9c391c967f7..2e1c0b71fe92 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -105,6 +105,7 @@ enum iio_event_type { + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, + IIO_EV_TYPE_FIFO_FLUSH, ++ IIO_EV_TYPE_TIME_SYNC, + }; + + enum iio_event_direction { +-- +2.25.1 + diff --git a/stm_iio_patches/4.19.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch b/stm_iio_patches/4.19.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch new file mode 100644 index 000000000000..818272619d90 --- /dev/null +++ b/stm_iio_patches/4.19.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch @@ -0,0 +1,49 @@ +From 9a63dacc4edd06ced07971c9f39e06f9f79b252a Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 2 Sep 2021 14:04:40 +0200 +Subject: [PATCH] Added iio type patch for STMEMS + +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 11 +++++++++++ + 1 file changed, 11 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index fdd81af..af20701 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -43,6 +43,14 @@ enum iio_chan_type { + IIO_ELECTRICALCONDUCTIVITY, + IIO_COUNT, + IIO_INDEX, ++ IIO_SIGN_MOTION, ++ IIO_STEP_DETECTOR, ++ IIO_STEP_COUNTER, ++ IIO_TILT, ++ IIO_TAP, ++ IIO_TAP_TAP, ++ IIO_WRIST_TILT_GESTURE, ++ IIO_GESTURE, + IIO_GRAVITY, + IIO_POSITIONRELATIVE, + IIO_PHASE, +@@ -103,6 +111,7 @@ enum iio_event_type { + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, ++ IIO_EV_TYPE_FIFO_FLUSH, + }; + + enum iio_event_direction { +@@ -110,6 +119,8 @@ enum iio_event_direction { + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, + IIO_EV_DIR_NONE, ++ IIO_EV_DIR_FIFO_EMPTY, ++ IIO_EV_DIR_FIFO_DATA, + }; + + #endif /* _UAPI_IIO_TYPES_H_ */ +-- +2.7.4 + diff --git a/stm_iio_patches/4.19.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch b/stm_iio_patches/4.19.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch new file mode 100644 index 000000000000..b0907ade2e4f --- /dev/null +++ b/stm_iio_patches/4.19.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch @@ -0,0 +1,33 @@ +From 608fba430cc3ab55d8c6d07d628365517b053596 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Thu, 2 Sep 2021 10:13:15 -0700 +Subject: [PATCH] Add STM sensors drivers to build system + +Signed-off-by: Denis Ciocca +--- + drivers/iio/Kconfig | 1 + + drivers/iio/Makefile | 1 + + 2 files changed, 2 insertions(+) + +diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig +index 5bd51853b..085045ac5 100644 +--- a/drivers/iio/Kconfig ++++ b/drivers/iio/Kconfig +@@ -94,5 +94,6 @@ source "drivers/iio/pressure/Kconfig" + source "drivers/iio/proximity/Kconfig" + source "drivers/iio/resolver/Kconfig" + source "drivers/iio/temperature/Kconfig" ++source "drivers/iio/stm/Kconfig" + + endif # IIO +diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile +index bff682ad1..3a002616b 100644 +--- a/drivers/iio/Makefile ++++ b/drivers/iio/Makefile +@@ -38,3 +38,4 @@ obj-y += proximity/ + obj-y += resolver/ + obj-y += temperature/ + obj-y += trigger/ ++obj-y += stm/ +-- +2.33.0 diff --git a/stm_iio_patches/4.19.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch b/stm_iio_patches/4.19.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch new file mode 100644 index 000000000000..42ef0a555e5a --- /dev/null +++ b/stm_iio_patches/4.19.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch @@ -0,0 +1,26 @@ +From 7f632aaebf1e9558d06dafcd56cd887e34f4c5a6 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Mon, 29 Nov 2021 15:15:48 -0800 +Subject: [PATCH 1/1] stm: add new iio event used for timesync logic + +Signed-off-by: Denis Ciocca +Change-Id: I05ec93ccdb9f4905d6a6c1900386332edf21fea2 +--- + include/uapi/linux/iio/types.h | 1 + + 1 file changed, 1 insertion(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index b9c391c967f7..2e1c0b71fe92 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -105,6 +105,7 @@ enum iio_event_type { + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, + IIO_EV_TYPE_FIFO_FLUSH, ++ IIO_EV_TYPE_TIME_SYNC, + }; + + enum iio_event_direction { +-- +2.25.1 + diff --git a/stm_iio_patches/4.9.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch b/stm_iio_patches/4.9.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch new file mode 100644 index 000000000000..43ccf8eb433e --- /dev/null +++ b/stm_iio_patches/4.9.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch @@ -0,0 +1,49 @@ +From 01ad47a59eadd9ce20dfb8a89907e141061b8bc9 Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 16 Sep 2021 15:35:14 +0200 +Subject: [PATCH 2/2] STMEMS: Added iio type patch for STMEMS + +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 11 +++++++++++ + 1 file changed, 11 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index 22e5e58..d227164 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -40,6 +40,14 @@ enum iio_chan_type { + IIO_PH, + IIO_UVINDEX, + IIO_ELECTRICALCONDUCTIVITY, ++ IIO_SIGN_MOTION, ++ IIO_STEP_DETECTOR, ++ IIO_STEP_COUNTER, ++ IIO_TILT, ++ IIO_TAP, ++ IIO_TAP_TAP, ++ IIO_WRIST_TILT_GESTURE, ++ IIO_GESTURE, + }; + + enum iio_modifier { +@@ -89,6 +97,7 @@ enum iio_event_type { + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, ++ IIO_EV_TYPE_FIFO_FLUSH, + }; + + enum iio_event_direction { +@@ -96,6 +105,8 @@ enum iio_event_direction { + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, + IIO_EV_DIR_NONE, ++ IIO_EV_DIR_FIFO_EMPTY, ++ IIO_EV_DIR_FIFO_DATA, + }; + + #endif /* _UAPI_IIO_TYPES_H_ */ +-- +2.7.4 + diff --git a/stm_iio_patches/4.9.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch b/stm_iio_patches/4.9.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch new file mode 100644 index 000000000000..f9806f1cdea6 --- /dev/null +++ b/stm_iio_patches/4.9.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch @@ -0,0 +1,34 @@ +From 6b3b528b789d3555952915bc436db6914cda5b8f Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 16 Sep 2021 15:33:29 +0200 +Subject: [PATCH 1/2] Added STMEMS sensor to build system + +Signed-off-by: mario tesi +--- + drivers/iio/Kconfig | 1 + + drivers/iio/Makefile | 1 + + 2 files changed, 2 insertions(+) + +diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig +index 6743b18..fbe089a 100644 +--- a/drivers/iio/Kconfig ++++ b/drivers/iio/Kconfig +@@ -90,5 +90,6 @@ source "drivers/iio/potentiometer/Kconfig" + source "drivers/iio/pressure/Kconfig" + source "drivers/iio/proximity/Kconfig" + source "drivers/iio/temperature/Kconfig" ++source "drivers/iio/stm/Kconfig" + + endif # IIO +diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile +index 87e4c43..8aff487 100644 +--- a/drivers/iio/Makefile ++++ b/drivers/iio/Makefile +@@ -33,3 +33,4 @@ obj-y += pressure/ + obj-y += proximity/ + obj-y += temperature/ + obj-y += trigger/ ++obj-y += stm/ +-- +2.7.4 + diff --git a/stm_iio_patches/4.9.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch b/stm_iio_patches/4.9.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch new file mode 100644 index 000000000000..01fbc0ce2d6d --- /dev/null +++ b/stm_iio_patches/4.9.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch @@ -0,0 +1,35 @@ +From 4de72e26919ffd46f3a5de43ca65f98f95d24e60 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Mon, 29 Nov 2021 15:15:48 -0800 +Subject: [PATCH] stm: add new iio event used for timesync logic + +Signed-off-by: Denis Ciocca +Change-Id: I05ec93ccdb9f4905d6a6c1900386332edf21fea2 +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 2 ++ + 1 file changed, 2 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index d227164..7127c1f 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -40,6 +40,7 @@ enum iio_chan_type { + IIO_PH, + IIO_UVINDEX, + IIO_ELECTRICALCONDUCTIVITY, ++ IIO_COUNT, + IIO_SIGN_MOTION, + IIO_STEP_DETECTOR, + IIO_STEP_COUNTER, +@@ -98,6 +99,7 @@ enum iio_event_type { + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, + IIO_EV_TYPE_FIFO_FLUSH, ++ IIO_EV_TYPE_TIME_SYNC, + }; + + enum iio_event_direction { +-- +2.7.4 + diff --git a/stm_iio_patches/5.10.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch b/stm_iio_patches/5.10.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch new file mode 100644 index 000000000000..818272619d90 --- /dev/null +++ b/stm_iio_patches/5.10.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch @@ -0,0 +1,49 @@ +From 9a63dacc4edd06ced07971c9f39e06f9f79b252a Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 2 Sep 2021 14:04:40 +0200 +Subject: [PATCH] Added iio type patch for STMEMS + +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 11 +++++++++++ + 1 file changed, 11 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index fdd81af..af20701 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -43,6 +43,14 @@ enum iio_chan_type { + IIO_ELECTRICALCONDUCTIVITY, + IIO_COUNT, + IIO_INDEX, ++ IIO_SIGN_MOTION, ++ IIO_STEP_DETECTOR, ++ IIO_STEP_COUNTER, ++ IIO_TILT, ++ IIO_TAP, ++ IIO_TAP_TAP, ++ IIO_WRIST_TILT_GESTURE, ++ IIO_GESTURE, + IIO_GRAVITY, + IIO_POSITIONRELATIVE, + IIO_PHASE, +@@ -103,6 +111,7 @@ enum iio_event_type { + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, ++ IIO_EV_TYPE_FIFO_FLUSH, + }; + + enum iio_event_direction { +@@ -110,6 +119,8 @@ enum iio_event_direction { + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, + IIO_EV_DIR_NONE, ++ IIO_EV_DIR_FIFO_EMPTY, ++ IIO_EV_DIR_FIFO_DATA, + }; + + #endif /* _UAPI_IIO_TYPES_H_ */ +-- +2.7.4 + diff --git a/stm_iio_patches/5.10.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch b/stm_iio_patches/5.10.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch new file mode 100644 index 000000000000..b0907ade2e4f --- /dev/null +++ b/stm_iio_patches/5.10.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch @@ -0,0 +1,33 @@ +From 608fba430cc3ab55d8c6d07d628365517b053596 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Thu, 2 Sep 2021 10:13:15 -0700 +Subject: [PATCH] Add STM sensors drivers to build system + +Signed-off-by: Denis Ciocca +--- + drivers/iio/Kconfig | 1 + + drivers/iio/Makefile | 1 + + 2 files changed, 2 insertions(+) + +diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig +index 5bd51853b..085045ac5 100644 +--- a/drivers/iio/Kconfig ++++ b/drivers/iio/Kconfig +@@ -94,5 +94,6 @@ source "drivers/iio/pressure/Kconfig" + source "drivers/iio/proximity/Kconfig" + source "drivers/iio/resolver/Kconfig" + source "drivers/iio/temperature/Kconfig" ++source "drivers/iio/stm/Kconfig" + + endif # IIO +diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile +index bff682ad1..3a002616b 100644 +--- a/drivers/iio/Makefile ++++ b/drivers/iio/Makefile +@@ -38,3 +38,4 @@ obj-y += proximity/ + obj-y += resolver/ + obj-y += temperature/ + obj-y += trigger/ ++obj-y += stm/ +-- +2.33.0 diff --git a/stm_iio_patches/5.10.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch b/stm_iio_patches/5.10.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch new file mode 100644 index 000000000000..42ef0a555e5a --- /dev/null +++ b/stm_iio_patches/5.10.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch @@ -0,0 +1,26 @@ +From 7f632aaebf1e9558d06dafcd56cd887e34f4c5a6 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Mon, 29 Nov 2021 15:15:48 -0800 +Subject: [PATCH 1/1] stm: add new iio event used for timesync logic + +Signed-off-by: Denis Ciocca +Change-Id: I05ec93ccdb9f4905d6a6c1900386332edf21fea2 +--- + include/uapi/linux/iio/types.h | 1 + + 1 file changed, 1 insertion(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index b9c391c967f7..2e1c0b71fe92 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -105,6 +105,7 @@ enum iio_event_type { + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, + IIO_EV_TYPE_FIFO_FLUSH, ++ IIO_EV_TYPE_TIME_SYNC, + }; + + enum iio_event_direction { +-- +2.25.1 + diff --git a/stm_iio_patches/5.15.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch b/stm_iio_patches/5.15.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch new file mode 100644 index 000000000000..820eee8b4488 --- /dev/null +++ b/stm_iio_patches/5.15.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch @@ -0,0 +1,49 @@ +From b33998f4362832bf9e49a8252d4ddba493ed399c Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 2 Sep 2021 14:04:40 +0200 +Subject: [PATCH 1/3] Added iio type patch for STMEMS + +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 11 +++++++++++ + 1 file changed, 11 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index 48c13147c0a8..b31c0fddba0a 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -43,6 +43,14 @@ enum iio_chan_type { + IIO_ELECTRICALCONDUCTIVITY, + IIO_COUNT, + IIO_INDEX, ++ IIO_SIGN_MOTION, ++ IIO_STEP_DETECTOR, ++ IIO_STEP_COUNTER, ++ IIO_TILT, ++ IIO_TAP, ++ IIO_TAP_TAP, ++ IIO_WRIST_TILT_GESTURE, ++ IIO_GESTURE, + IIO_GRAVITY, + IIO_POSITIONRELATIVE, + IIO_PHASE, +@@ -104,6 +112,7 @@ enum iio_event_type { + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, ++ IIO_EV_TYPE_FIFO_FLUSH, + }; + + enum iio_event_direction { +@@ -111,6 +120,8 @@ enum iio_event_direction { + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, + IIO_EV_DIR_NONE, ++ IIO_EV_DIR_FIFO_EMPTY, ++ IIO_EV_DIR_FIFO_DATA, + }; + + #endif /* _UAPI_IIO_TYPES_H_ */ +-- +2.25.1 + diff --git a/stm_iio_patches/5.15.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch b/stm_iio_patches/5.15.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch new file mode 100644 index 000000000000..abe5582ee40b --- /dev/null +++ b/stm_iio_patches/5.15.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch @@ -0,0 +1,34 @@ +From 7c1c2733aab26eb55a95540696d924c4b86cef22 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Thu, 2 Sep 2021 10:13:15 -0700 +Subject: [PATCH 2/3] Add STM sensors drivers to build system + +Signed-off-by: Denis Ciocca +--- + drivers/iio/Kconfig | 1 + + drivers/iio/Makefile | 1 + + 2 files changed, 2 insertions(+) + +diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig +index 2334ad249b46..ebb5fe2f4473 100644 +--- a/drivers/iio/Kconfig ++++ b/drivers/iio/Kconfig +@@ -97,5 +97,6 @@ source "drivers/iio/pressure/Kconfig" + source "drivers/iio/proximity/Kconfig" + source "drivers/iio/resolver/Kconfig" + source "drivers/iio/temperature/Kconfig" ++source "drivers/iio/stm/Kconfig" + + endif # IIO +diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile +index 65e39bd4f934..c96ed231d22a 100644 +--- a/drivers/iio/Makefile ++++ b/drivers/iio/Makefile +@@ -41,3 +41,4 @@ obj-y += resolver/ + obj-y += temperature/ + obj-y += test/ + obj-y += trigger/ ++obj-y += stm/ +-- +2.25.1 + diff --git a/stm_iio_patches/5.15.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch b/stm_iio_patches/5.15.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch new file mode 100644 index 000000000000..536f226d5bd3 --- /dev/null +++ b/stm_iio_patches/5.15.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch @@ -0,0 +1,26 @@ +From c302b734296ff72b3de80928294dbafe7a9252dd Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Mon, 29 Nov 2021 15:15:48 -0800 +Subject: [PATCH 3/3] stm: add new iio event used for timesync logic + +Signed-off-by: Denis Ciocca +Change-Id: I05ec93ccdb9f4905d6a6c1900386332edf21fea2 +--- + include/uapi/linux/iio/types.h | 1 + + 1 file changed, 1 insertion(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index b31c0fddba0a..95f336b07fc2 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -113,6 +113,7 @@ enum iio_event_type { + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, + IIO_EV_TYPE_FIFO_FLUSH, ++ IIO_EV_TYPE_TIME_SYNC, + }; + + enum iio_event_direction { +-- +2.25.1 + diff --git a/stm_iio_patches/5.4.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch b/stm_iio_patches/5.4.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch new file mode 100644 index 000000000000..818272619d90 --- /dev/null +++ b/stm_iio_patches/5.4.y/0001-stm-Added-iio-type-patch-for-STMEMS.patch @@ -0,0 +1,49 @@ +From 9a63dacc4edd06ced07971c9f39e06f9f79b252a Mon Sep 17 00:00:00 2001 +From: mario tesi +Date: Thu, 2 Sep 2021 14:04:40 +0200 +Subject: [PATCH] Added iio type patch for STMEMS + +Signed-off-by: mario tesi +--- + include/uapi/linux/iio/types.h | 11 +++++++++++ + 1 file changed, 11 insertions(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index fdd81af..af20701 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -43,6 +43,14 @@ enum iio_chan_type { + IIO_ELECTRICALCONDUCTIVITY, + IIO_COUNT, + IIO_INDEX, ++ IIO_SIGN_MOTION, ++ IIO_STEP_DETECTOR, ++ IIO_STEP_COUNTER, ++ IIO_TILT, ++ IIO_TAP, ++ IIO_TAP_TAP, ++ IIO_WRIST_TILT_GESTURE, ++ IIO_GESTURE, + IIO_GRAVITY, + IIO_POSITIONRELATIVE, + IIO_PHASE, +@@ -103,6 +111,7 @@ enum iio_event_type { + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, ++ IIO_EV_TYPE_FIFO_FLUSH, + }; + + enum iio_event_direction { +@@ -110,6 +119,8 @@ enum iio_event_direction { + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, + IIO_EV_DIR_NONE, ++ IIO_EV_DIR_FIFO_EMPTY, ++ IIO_EV_DIR_FIFO_DATA, + }; + + #endif /* _UAPI_IIO_TYPES_H_ */ +-- +2.7.4 + diff --git a/stm_iio_patches/5.4.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch b/stm_iio_patches/5.4.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch new file mode 100644 index 000000000000..b0907ade2e4f --- /dev/null +++ b/stm_iio_patches/5.4.y/0002-stm-Add-STM-sensors-drivers-to-build-system.patch @@ -0,0 +1,33 @@ +From 608fba430cc3ab55d8c6d07d628365517b053596 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Thu, 2 Sep 2021 10:13:15 -0700 +Subject: [PATCH] Add STM sensors drivers to build system + +Signed-off-by: Denis Ciocca +--- + drivers/iio/Kconfig | 1 + + drivers/iio/Makefile | 1 + + 2 files changed, 2 insertions(+) + +diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig +index 5bd51853b..085045ac5 100644 +--- a/drivers/iio/Kconfig ++++ b/drivers/iio/Kconfig +@@ -94,5 +94,6 @@ source "drivers/iio/pressure/Kconfig" + source "drivers/iio/proximity/Kconfig" + source "drivers/iio/resolver/Kconfig" + source "drivers/iio/temperature/Kconfig" ++source "drivers/iio/stm/Kconfig" + + endif # IIO +diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile +index bff682ad1..3a002616b 100644 +--- a/drivers/iio/Makefile ++++ b/drivers/iio/Makefile +@@ -38,3 +38,4 @@ obj-y += proximity/ + obj-y += resolver/ + obj-y += temperature/ + obj-y += trigger/ ++obj-y += stm/ +-- +2.33.0 diff --git a/stm_iio_patches/5.4.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch b/stm_iio_patches/5.4.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch new file mode 100644 index 000000000000..42ef0a555e5a --- /dev/null +++ b/stm_iio_patches/5.4.y/0003-stm-add-new-iio-event-used-for-timesync-logic.patch @@ -0,0 +1,26 @@ +From 7f632aaebf1e9558d06dafcd56cd887e34f4c5a6 Mon Sep 17 00:00:00 2001 +From: Denis Ciocca +Date: Mon, 29 Nov 2021 15:15:48 -0800 +Subject: [PATCH 1/1] stm: add new iio event used for timesync logic + +Signed-off-by: Denis Ciocca +Change-Id: I05ec93ccdb9f4905d6a6c1900386332edf21fea2 +--- + include/uapi/linux/iio/types.h | 1 + + 1 file changed, 1 insertion(+) + +diff --git a/include/uapi/linux/iio/types.h b/include/uapi/linux/iio/types.h +index b9c391c967f7..2e1c0b71fe92 100644 +--- a/include/uapi/linux/iio/types.h ++++ b/include/uapi/linux/iio/types.h +@@ -105,6 +105,7 @@ enum iio_event_type { + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_CHANGE, + IIO_EV_TYPE_FIFO_FLUSH, ++ IIO_EV_TYPE_TIME_SYNC, + }; + + enum iio_event_direction { +-- +2.25.1 +