meta-agl-refhw-gen3/recipes-kernel/linux/files/0002-add-st_asm330lhh-driver.patch

   1 ---
   2  drivers/iio/imu/st_asm330lhh/Kconfig               |  22 +
   3  drivers/iio/imu/st_asm330lhh/Makefile              |   5 +
   4  drivers/iio/imu/st_asm330lhh/README.md             | 201 +++++
   5  drivers/iio/imu/st_asm330lhh/st_asm330lhh.h        | 247 ++++++
   6  drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c | 540 ++++++++++++++
   7  drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c   | 824 +++++++++++++++++++++
   8  drivers/iio/imu/st_asm330lhh/st_asm330lhh_i2c.c    |  94 +++
   9  drivers/iio/imu/st_asm330lhh/st_asm330lhh_spi.c    | 109 +++
  10  8 files changed, 2042 insertions(+)
  11  create mode 100644 drivers/iio/imu/st_asm330lhh/Kconfig
  12  create mode 100644 drivers/iio/imu/st_asm330lhh/Makefile
  13  create mode 100644 drivers/iio/imu/st_asm330lhh/README.md
  14  create mode 100644 drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
  15  create mode 100644 drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
  16  create mode 100644 drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
  17  create mode 100644 drivers/iio/imu/st_asm330lhh/st_asm330lhh_i2c.c
  18  create mode 100644 drivers/iio/imu/st_asm330lhh/st_asm330lhh_spi.c
  19
  20 diff --git a/drivers/iio/imu/st_asm330lhh/Kconfig b/drivers/iio/imu/st_asm330lhh/Kconfig
  21 new file mode 100644
  22 index 0000000..0e8920e
  23 --- /dev/null
  24 +++ b/drivers/iio/imu/st_asm330lhh/Kconfig
  25 @@ -0,0 +1,22 @@
  26 +config IIO_ST_ASM330LHH
  27 +       tristate "STMicroelectronics ASM330LHH sensor"
  28 +       depends on (I2C || SPI)
  29 +       select IIO_BUFFER
  30 +       select IIO_KFIFO_BUF
  31 +       select IIO_ST_ASM330LHH_I2C if (I2C)
  32 +       select IIO_ST_ASM330LHH_SPI if (SPI_MASTER)
  33 +       help
  34 +         Say yes here to build support for STMicroelectronics ASM330LHH imu
  35 +         sensor.
  36 +
  37 +         To compile this driver as a module, choose M here: the module
  38 +         will be called st_asm330lhh.
  39 +
  40 +config IIO_ST_ASM330LHH_I2C
  41 +       tristate
  42 +       depends on IIO_ST_ASM330LHH
  43 +
  44 +config IIO_ST_ASM330LHH_SPI
  45 +       tristate
  46 +       depends on IIO_ST_ASM330LHH
  47 +
  48 diff --git a/drivers/iio/imu/st_asm330lhh/Makefile b/drivers/iio/imu/st_asm330lhh/Makefile
  49 new file mode 100644
  50 index 0000000..7af80de
  51 --- /dev/null
  52 +++ b/drivers/iio/imu/st_asm330lhh/Makefile
  53 @@ -0,0 +1,5 @@
  54 +st_asm330lhh-y := st_asm330lhh_core.o st_asm330lhh_buffer.o
  55 +
  56 +obj-$(CONFIG_IIO_ST_ASM330LHH) += st_asm330lhh.o
  57 +obj-$(CONFIG_IIO_ST_ASM330LHH_I2C) += st_asm330lhh_i2c.o
  58 +obj-$(CONFIG_IIO_ST_ASM330LHH_SPI) += st_asm330lhh_spi.o
  59 diff --git a/drivers/iio/imu/st_asm330lhh/README.md b/drivers/iio/imu/st_asm330lhh/README.md
  60 new file mode 100644
  61 index 0000000..d471530
  62 --- /dev/null
  63 +++ b/drivers/iio/imu/st_asm330lhh/README.md
  64 @@ -0,0 +1,201 @@
  65 +Index
  66 +=======
  67 +       * Introduction
  68 +       * Driver Integration details
  69 +       * Android SensorHAL integration
  70 +       * Linux SensorHAL integration
  71 +       * More information
  72 +       * Copyright
  73 +
  74 +
  75 +Introduction
  76 +==============
  77 +This repository contains asm330lhh IMU STMicroelectronics MEMS sensor linux driver support for kernel version 4.14.
  78 +
  79 +Data collected by asm330lhh STM sensor are pushed to userland through the kernel buffers of Linux IIO framework. User space applications can get sensor events by reading the related IIO devices created in the /dev directory (*/dev/iio{x}*). Please see [IIO][1] for more information.
  80 +
  81 +Asm330lhh IMU STM MEMS sensor support *I2C/SPI* digital interface. Please refer to [I2C][2] and [SPI][3] for detailed documentation.
  82 +
  83 +The STM Hardware Abstraction Layer (*HAL*) defines a standard interface for STM sensors allowing Android to be agnostic about low level driver implementation. The HAL library is packaged into modules (.so) file and loaded by the Android or Linux system at the appropriate time. For more information see [AOSP HAL Interface](https://source.android.com/devices/sensors/hal-interface.html)
  84 +
  85 +STM Sensor HAL is leaning on [Linux IIO framework](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/iio) to gather data from sensor device drivers and to forward samples to the Android Framework
  86 +
  87 +Driver Integration details
  88 +=====================
  89 +
  90 +In order to explain how to integrate Asm330lhh IMU STM sensor into the kernel, please consider the following example
  91 +
  92 +### Source code integration
  93 +
  94 +> * Copy driver source code into your linux kernel target directory (e.g. *drivers/iio/imu*)
  95 +> * Edit related Kconfig (e.g. *drivers/iio/imu/Kconfig*) adding *ASM330LHH* support:
  96 +
  97 +>         source "drivers/iio/imu/st_asm330lhh/Kconfig"
  98 +
  99 +> * Edit related Makefile (e.g. *drivers/iio/imu/Makefile*) adding the following line:
 100 +
 101 +>         obj-y += st_asm330lhh/
 102 +
 103 +### Device Tree configuration
 104 +
 105 +> To enable driver probing, add the asm330lhh node to the platform device tree as described below.
 106 +
 107 +> **Required properties:**
 108 +
 109 +> *- compatible*: "st,asm330lhh"
 110 +
 111 +> *- reg*: the I2C address or SPI chip select the device will respond to
 112 +
 113 +> *- interrupt-parent*: phandle to the parent interrupt controller as documented in [interrupts][4]
 114 +
 115 +> *- interrupts*: interrupt mapping for IRQ as documented in [interrupts][4]
 116 +>
 117 +>**Recommended properties for SPI bus usage:**
 118 +
 119 +> *- spi-max-frequency*: maximum SPI bus frequency as documented in [SPI][3]
 120 +>
 121 +> **Optional properties:**
 122 +
 123 +> *- st,drdy-int-pin*: MEMS sensor interrupt line to use (default 1)
 124 +
 125 +> I2C example (based on Raspberry PI 3):
 126 +
 127 +>              &i2c0 {
 128 +>                      status = "ok";
 129 +>                      #address-cells = <0x1>;
 130 +>                      #size-cells = <0x0>;
 131 +>                      asm330lhh@6b {
 132 +>                              compatible = "st,asm330lhh";
 133 +>                              reg = <0x6b>;
 134 +>                              interrupt-parent = <&gpio>;
 135 +>                              interrupts = <26 IRQ_TYPE_EDGE_RISING>;
 136 +>              };
 137 +
 138 +> SPI example (based on Raspberry PI 3):
 139 +
 140 +>              &spi0 {
 141 +>                      status = "ok";
 142 +>                      #address-cells = <0x1>;
 143 +>                      #size-cells = <0x0>;
 144 +>                      asm330lhh@0 {
 145 +>                              spi-max-frequency = <500000>;
 146 +>                              compatible = "st,asm330lhh";
 147 +>                              reg = <0>;
 148 +>                              interrupt-parent = <&gpio>;
 149 +>                              interrupts = <26 IRQ_TYPE_EDGE_RISING>;
 150 +>                      };
 151 +
 152 +### Kernel configuration
 153 +
 154 +Configure kernel with *make menuconfig* (alternatively use *make xconfig* or *make qconfig*)
 155 +
 156 +>              Device Drivers  --->
 157 +>                      <M> Industrial I/O support  --->
 158 +>                              Inertial measurement units  --->
 159 +>                              <M>   STMicroelectronics ASM330LHH sensor  --->
 160 +
 161 +
 162 +Android SensorHAL integration
 163 +==============
 164 +
 165 +STM Sensor HAL is written in *C++* language using object-oriented design. For each hw sensor there is a custom class file (*Accelerometer.cpp*, *Gyroscope.cpp*) which extends the common base class (*SensorBase.cpp*).
 166 +
 167 +Copy the HAL source code into *<AOSP_DIR\>/hardware/STMicroelectronics/SensorHAL_IIO* folder. During building process Android will include automatically the SensorHAL Android.mk.
 168 +In *<AOSP_DIR\>/device/<vendor\>/<board\>/device.mk* add package build information:
 169 +
 170 +       PRODUCT_PACKAGES += sensors.{TARGET_BOARD_PLATFORM}
 171 +
 172 +       Note: device.mk can not read $(TARGET_BOARD_PLATFORM) variable, read and replace the value from your BoardConfig.mk (e.g. PRODUCT_PACKAGES += sensors.msm8974 for Nexus 5)
 173 +
 174 +To compile the SensorHAL_IIO just build AOSP source code from *$TOP* folder
 175 +
 176 +       $ cd <AOSP_DIR>
 177 +       $ source build/envsetup.sh
 178 +       $ lunch <select target platform>
 179 +       $ make V=99
 180 +
 181 +The compiled library will be placed in *<AOSP_DIR\>/out/target/product/<board\>/system/vendor/lib/hw/sensor.{TARGET_BOARD_PLATFORM}.so*
 182 +
 183 +To configure sensor the Sensor HAL IIO use mm utility from HAL root folder
 184 +
 185 +    since Android 7
 186 +       $mm sensors-defconfig (default configuration)
 187 +       $mm sensors-menuconfig
 188 +
 189 +    after Android 7
 190 +    make -f Makefile_config sensors-defconfig (default configuration)
 191 +    make -f Makefile_config sensors-menuconfig
 192 +
 193 +Linux SensorHAL integration
 194 +==============
 195 +
 196 +Linux Sensor HAL share the same source code of Android Sensor HAL. Before compiling the Linux Sensor HAL IIO
 197 +you need to follow the same procedure listed in previous chapter "Android SensorHAL integration"
 198 +To cross compile Linux Sensor HAL must export the following shell variables
 199 +
 200 +>   export AOSP_DIR=<AOSP_DIR>
 201 +>   export ARCH=<your target architecture>
 202 +>   export CROSS_COMPILE=<toolchain for your target>
 203 +
 204 +then in *<AOSP_DIR\>/hardware/STMicroelectronics/SensorHAL_IIO* folder type
 205 +>   make
 206 +
 207 +it will produce a SensorHAL.so file containing the library.
 208 +In relative pat Documentation/LinuxHal/ there are some examples explaining how to use Linux Sensor HAL
 209 +
 210 +Copyright
 211 +========
 212 +Copyright (C) 2017 STMicroelectronics
 213 +
 214 +Licensed under the Apache License, Version 2.0 (the "License");
 215 +you may not use this file except in compliance with the License.
 216 +You may obtain a copy of the License at
 217 +
 218 +http://www.apache.org/licenses/LICENSE-2.0
 219 +
 220 +Unless required by applicable law or agreed to in writing, software
 221 +distributed under the License is distributed on an "AS IS" BASIS,
 222 +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 223 +See the License for the specific language governing permissions and
 224 +limitations under the License.
 225 +
 226 +
 227 +More Information
 228 +=================
 229 +[http://st.com](http://st.com)
 230 +
 231 +[https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/iio](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/input)
 232 +
 233 +[https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/i2c](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/i2c)
 234 +
 235 +[https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/spi](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/spi)
 236 +
 237 +[https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/devicetree/bings/interrupt-controller/interrupts.txt](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/devicetree/bindings/interrupt-controller/interrupts.txt)
 238 +
 239 +
 240 +Copyright Driver
 241 +===========
 242 +Copyright (C) 2017 STMicroelectronics
 243 +
 244 +This software is distributed under the GNU General Public License - see the accompanying COPYING file for more details.
 245 +
 246 +[1]: https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/Documentation/iio/iio_configfs.txt "IIO"
 247 +[2]: https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/i2c "I2C"
 248 +[3]: https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/spi "SPI"
 249 +[4]: https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/Documentation/devicetree/bindings/interrupt-controller/interrupts.txt "interrupts"
 250 +
 251 +Copyright SensorHAL
 252 +========
 253 +Copyright (C) 2017 STMicroelectronics
 254 +
 255 +Licensed under the Apache License, Version 2.0 (the "License");
 256 +you may not use this file except in compliance with the License.
 257 +You may obtain a copy of the License at
 258 +
 259 +http://www.apache.org/licenses/LICENSE-2.0
 260 +
 261 +Unless required by applicable law or agreed to in writing, software
 262 +distributed under the License is distributed on an "AS IS" BASIS,
 263 +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 264 +See the License for the specific language governing permissions and
 265 +limitations under the License.
 266 diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h b/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
 267 new file mode 100644
 268 index 0000000..a2f6e02
 269 --- /dev/null
 270 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
 271 @@ -0,0 +1,247 @@
 272 +/*
 273 + * STMicroelectronics st_asm330lhh sensor driver
 274 + *
 275 + * Copyright 2018 STMicroelectronics Inc.
 276 + *
 277 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
 278 + *
 279 + * Licensed under the GPL-2.
 280 + */
 281 +
 282 +#ifndef ST_ASM330LHH_H
 283 +#define ST_ASM330LHH_H
 284 +
 285 +#include <linux/device.h>
 286 +#include <linux/iio/iio.h>
 287 +
 288 +#define ST_ASM330LHH_REVISION          "2.0.1"
 289 +#define ST_ASM330LHH_PATCH             "2"
 290 +
 291 +#define ST_ASM330LHH_VERSION           "v"     \
 292 +       ST_ASM330LHH_REVISION                   \
 293 +       "-"                                     \
 294 +       ST_ASM330LHH_PATCH
 295 +
 296 +#define ST_ASM330LHH_DEV_NAME          "asm330lhh"
 297 +
 298 +#define ST_ASM330LHH_SAMPLE_SIZE       6
 299 +#define ST_ASM330LHH_TS_SAMPLE_SIZE    4
 300 +#define ST_ASM330LHH_TAG_SIZE          1
 301 +#define ST_ASM330LHH_FIFO_SAMPLE_SIZE  (ST_ASM330LHH_SAMPLE_SIZE + \
 302 +                                        ST_ASM330LHH_TAG_SIZE)
 303 +#define ST_ASM330LHH_MAX_FIFO_DEPTH    416
 304 +
 305 +#define ST_ASM330LHH_REG_FIFO_BATCH_ADDR       0x09
 306 +#define ST_ASM330LHH_REG_FIFO_CTRL4_ADDR       0x0a
 307 +#define ST_ASM330LHH_REG_STATUS_ADDR           0x1e
 308 +#define ST_ASM330LHH_REG_STATUS_TDA            BIT(2)
 309 +#define ST_ASM330LHH_REG_OUT_TEMP_L_ADDR       0x20
 310 +#define ST_ASM330LHH_REG_OUT_TEMP_H_ADDR       0x21
 311 +
 312 +#define ST_ASM330LHH_MAX_ODR                   416
 313 +
 314 +/* Define Custom events for FIFO flush */
 315 +#define CUSTOM_IIO_EV_DIR_FIFO_EMPTY (IIO_EV_DIR_NONE + 1)
 316 +#define CUSTOM_IIO_EV_DIR_FIFO_DATA (IIO_EV_DIR_NONE + 2)
 317 +#define CUSTOM_IIO_EV_TYPE_FIFO_FLUSH (IIO_EV_TYPE_CHANGE + 1)
 318 +
 319 +#define ST_ASM330LHH_CHANNEL(chan_type, addr, mod, ch2, scan_idx,      \
 320 +                          rb, sb, sg)                                  \
 321 +{                                                                      \
 322 +       .type = chan_type,                                              \
 323 +       .address = addr,                                                \
 324 +       .modified = mod,                                                \
 325 +       .channel2 = ch2,                                                \
 326 +       .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |                  \
 327 +                             BIT(IIO_CHAN_INFO_SCALE),                 \
 328 +       .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),        \
 329 +       .scan_index = scan_idx,                                         \
 330 +       .scan_type = {                                                  \
 331 +               .sign = sg,                                             \
 332 +               .realbits = rb,                                         \
 333 +               .storagebits = sb,                                      \
 334 +               .endianness = IIO_LE,                                   \
 335 +       },                                                              \
 336 +}
 337 +
 338 +static const struct iio_event_spec st_asm330lhh_flush_event = {
 339 +       .type = CUSTOM_IIO_EV_TYPE_FIFO_FLUSH,
 340 +       .dir = IIO_EV_DIR_EITHER,
 341 +};
 342 +
 343 +#define ST_ASM330LHH_FLUSH_CHANNEL(dtype)              \
 344 +{                                                      \
 345 +       .type = dtype,                                  \
 346 +       .modified = 0,                                  \
 347 +       .scan_index = -1,                               \
 348 +       .indexed = -1,                                  \
 349 +       .event_spec = &st_asm330lhh_flush_event,        \
 350 +       .num_event_specs = 1,                           \
 351 +}
 352 +
 353 +#define ST_ASM330LHH_RX_MAX_LENGTH     8
 354 +#define ST_ASM330LHH_TX_MAX_LENGTH     8
 355 +
 356 +struct st_asm330lhh_transfer_buffer {
 357 +       u8 rx_buf[ST_ASM330LHH_RX_MAX_LENGTH];
 358 +       u8 tx_buf[ST_ASM330LHH_TX_MAX_LENGTH] ____cacheline_aligned;
 359 +};
 360 +
 361 +struct st_asm330lhh_transfer_function {
 362 +       int (*read)(struct device *dev, u8 addr, int len, u8 *data);
 363 +       int (*write)(struct device *dev, u8 addr, int len, u8 *data);
 364 +};
 365 +
 366 +struct st_asm330lhh_reg {
 367 +       u8 addr;
 368 +       u8 mask;
 369 +};
 370 +
 371 +struct st_asm330lhh_odr {
 372 +       u16 hz;
 373 +       u8 val;
 374 +};
 375 +
 376 +#define ST_ASM330LHH_ODR_LIST_SIZE     7
 377 +struct st_asm330lhh_odr_table_entry {
 378 +       struct st_asm330lhh_reg reg;
 379 +       struct st_asm330lhh_odr odr_avl[ST_ASM330LHH_ODR_LIST_SIZE];
 380 +};
 381 +
 382 +struct st_asm330lhh_fs {
 383 +       u32 gain;
 384 +       u8 val;
 385 +};
 386 +
 387 +#define ST_ASM330LHH_FS_ACC_LIST_SIZE          4
 388 +#define ST_ASM330LHH_FS_GYRO_LIST_SIZE         6
 389 +#define ST_ASM330LHH_FS_TEMP_LIST_SIZE         1
 390 +#define ST_ASM330LHH_FS_LIST_SIZE              6
 391 +struct st_asm330lhh_fs_table_entry {
 392 +       u32 size;
 393 +       struct st_asm330lhh_reg reg;
 394 +       struct st_asm330lhh_fs fs_avl[ST_ASM330LHH_FS_LIST_SIZE];
 395 +};
 396 +
 397 +enum st_asm330lhh_sensor_id {
 398 +       ST_ASM330LHH_ID_ACC,
 399 +       ST_ASM330LHH_ID_GYRO,
 400 +       ST_ASM330LHH_ID_TEMP,
 401 +       ST_ASM330LHH_ID_MAX,
 402 +};
 403 +
 404 +enum st_asm330lhh_fifo_mode {
 405 +       ST_ASM330LHH_FIFO_BYPASS = 0x0,
 406 +       ST_ASM330LHH_FIFO_CONT = 0x6,
 407 +};
 408 +
 409 +enum {
 410 +       ST_ASM330LHH_HW_FLUSH,
 411 +       ST_ASM330LHH_HW_OPERATIONAL,
 412 +};
 413 +
 414 +/**
 415 + * struct st_asm330lhh_sensor - ST IMU sensor instance
 416 + * @id: Sensor identifier.
 417 + * @hw: Pointer to instance of struct st_asm330lhh_hw.
 418 + * @gain: Configured sensor sensitivity.
 419 + * @odr: Output data rate of the sensor [Hz].
 420 + * @watermark: Sensor watermark level.
 421 + * @batch_mask: Sensor mask for FIFO batching register
 422 + */
 423 +struct st_asm330lhh_sensor {
 424 +       enum st_asm330lhh_sensor_id id;
 425 +       struct st_asm330lhh_hw *hw;
 426 +
 427 +       u32 gain;
 428 +       u16 odr;
 429 +       u32 offset;
 430 +
 431 +       __le16 old_data;
 432 +
 433 +       u8 std_samples;
 434 +       u8 std_level;
 435 +
 436 +       u16 watermark;
 437 +       u8 batch_mask;
 438 +       u8 batch_addr;
 439 +};
 440 +
 441 +/**
 442 + * struct st_asm330lhh_hw - ST IMU MEMS hw instance
 443 + * @dev: Pointer to instance of struct device (I2C or SPI).
 444 + * @irq: Device interrupt line (I2C or SPI).
 445 + * @lock: Mutex to protect read and write operations.
 446 + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
 447 + * @fifo_mode: FIFO operating mode supported by the device.
 448 + * @state: hw operational state.
 449 + * @enable_mask: Enabled sensor bitmask.
 450 + * @ts_offset: Hw timestamp offset.
 451 + * @hw_val: Latest hw timestamp value.
 452 + * @hw_val_old: The hw saved timestamp value.
 453 + * @hw_ts: Latest hw timestamp from the sensor.
 454 + * @hw_ts_high: MSB of HW timestamp for rollover mamagenemt.
 455 + * @delta_ts: Delta time between two consecutive interrupts.
 456 + * @ts: Latest timestamp from irq handler.
 457 + * @tsample: Sample timestamp.
 458 + * @hw_ts_old: Prev. timestamp value.
 459 + * @delta_hw_ts: Estimated delta hw timestamp.
 460 + * @odr: Timestamp sample ODR.
 461 + * @iio_devs: Pointers to acc/gyro iio_dev instances.
 462 + * @tf: Transfer function structure used by I/O operations.
 463 + * @tb: Transfer buffers used by SPI I/O operations.
 464 + */
 465 +struct st_asm330lhh_hw {
 466 +       struct device *dev;
 467 +       int irq;
 468 +
 469 +       struct mutex lock;
 470 +       struct mutex fifo_lock;
 471 +
 472 +       enum st_asm330lhh_fifo_mode fifo_mode;
 473 +       unsigned long state;
 474 +       u8 enable_mask;
 475 +
 476 +       s64 ts_offset;
 477 +       u32 hw_val;
 478 +       u32 hw_val_old;
 479 +       s64 hw_ts;
 480 +       s64 hw_ts_high;
 481 +       s64 delta_ts;
 482 +       s64 ts;
 483 +       s64 tsample;
 484 +       s64 hw_ts_old;
 485 +       s64 delta_hw_ts;
 486 +
 487 +       u16 odr;
 488 +
 489 +       struct iio_dev *iio_devs[ST_ASM330LHH_ID_MAX];
 490 +
 491 +       const struct st_asm330lhh_transfer_function *tf;
 492 +       struct st_asm330lhh_transfer_buffer tb;
 493 +};
 494 +
 495 +extern const struct dev_pm_ops st_asm330lhh_pm_ops;
 496 +
 497 +int st_asm330lhh_probe(struct device *dev, int irq,
 498 +                      const struct st_asm330lhh_transfer_function *tf_ops);
 499 +int st_asm330lhh_sensor_set_enable(struct st_asm330lhh_sensor *sensor,
 500 +                                  bool enable);
 501 +int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw);
 502 +int st_asm330lhh_write_with_mask(struct st_asm330lhh_hw *hw, u8 addr, u8 mask,
 503 +                                u8 val);
 504 +int st_asm330lhh_get_odr_val(enum st_asm330lhh_sensor_id id, u16 odr, u8 *val);
 505 +ssize_t st_asm330lhh_flush_fifo(struct device *dev,
 506 +                               struct device_attribute *attr,
 507 +                               const char *buf, size_t size);
 508 +ssize_t st_asm330lhh_get_max_watermark(struct device *dev,
 509 +                                      struct device_attribute *attr, char *buf);
 510 +ssize_t st_asm330lhh_get_watermark(struct device *dev,
 511 +                                  struct device_attribute *attr, char *buf);
 512 +ssize_t st_asm330lhh_set_watermark(struct device *dev,
 513 +                                  struct device_attribute *attr,
 514 +                                  const char *buf, size_t size);
 515 +int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw,
 516 +                              enum st_asm330lhh_fifo_mode fifo_mode);
 517 +int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw);
 518 +#endif /* ST_ASM330LHH_H */
 519 diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
 520 new file mode 100644
 521 index 0000000..c414a06
 522 --- /dev/null
 523 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
 524 @@ -0,0 +1,540 @@
 525 +/*
 526 + * STMicroelectronics st_asm330lhh FIFO buffer library driver
 527 + *
 528 + * Copyright 2018 STMicroelectronics Inc.
 529 + *
 530 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
 531 + *
 532 + * Licensed under the GPL-2.
 533 + */
 534 +#include <linux/module.h>
 535 +#include <linux/interrupt.h>
 536 +#include <linux/irq.h>
 537 +#include <linux/iio/iio.h>
 538 +#include <linux/iio/kfifo_buf.h>
 539 +#include <linux/iio/events.h>
 540 +#include <linux/iio/buffer.h>
 541 +#include <asm/unaligned.h>
 542 +#include <linux/of.h>
 543 +
 544 +#include "st_asm330lhh.h"
 545 +
 546 +#define ST_ASM330LHH_REG_FIFO_THL_ADDR         0x07
 547 +#define ST_ASM330LHH_REG_FIFO_LEN_MASK         GENMASK(8, 0)
 548 +#define ST_ASM330LHH_REG_FIFO_MODE_MASK                GENMASK(2, 0)
 549 +#define ST_ASM330LHH_REG_DEC_TS_MASK           GENMASK(7, 6)
 550 +#define ST_ASM330LHH_REG_HLACTIVE_ADDR         0x12
 551 +#define ST_ASM330LHH_REG_HLACTIVE_MASK         BIT(5)
 552 +#define ST_ASM330LHH_REG_PP_OD_ADDR            0x12
 553 +#define ST_ASM330LHH_REG_PP_OD_MASK            BIT(4)
 554 +#define ST_ASM330LHH_REG_FIFO_DIFFL_ADDR       0x3a
 555 +#define ST_ASM330LHH_REG_TS0_ADDR              0x40
 556 +#define ST_ASM330LHH_REG_TS2_ADDR              0x42
 557 +#define ST_ASM330LHH_REG_FIFO_OUT_TAG_ADDR     0x78
 558 +#define ST_ASM330LHH_GYRO_TAG                  0x01
 559 +#define ST_ASM330LHH_ACC_TAG                   0x02
 560 +#define ST_ASM330LHH_TS_TAG                    0x04
 561 +
 562 +#define ST_ASM330LHH_TS_DELTA_NS               25000ULL /* 25us/LSB */
 563 +
 564 +static inline s64 st_asm330lhh_get_time_ns(void)
 565 +{
 566 +       struct timespec ts;
 567 +
 568 +       get_monotonic_boottime(&ts);
 569 +       return timespec_to_ns(&ts);
 570 +}
 571 +
 572 +#define ST_ASM330LHH_EWMA_LEVEL                        120
 573 +#define ST_ASM330LHH_EWMA_DIV                  128
 574 +static inline s64 st_asm330lhh_ewma(s64 old, s64 new, int weight)
 575 +{
 576 +       s64 diff, incr;
 577 +
 578 +       diff = new - old;
 579 +       incr = div_s64((ST_ASM330LHH_EWMA_DIV - weight) * diff,
 580 +                      ST_ASM330LHH_EWMA_DIV);
 581 +
 582 +       return old + incr;
 583 +}
 584 +
 585 +static inline int st_asm330lhh_reset_hwts(struct st_asm330lhh_hw *hw)
 586 +{
 587 +       u8 data = 0xaa;
 588 +
 589 +       hw->ts = st_asm330lhh_get_time_ns();
 590 +       hw->ts_offset = hw->ts;
 591 +       hw->hw_ts_old = 0ull;
 592 +       hw->tsample = 0ull;
 593 +       hw->hw_ts_high = 0ull;
 594 +       hw->hw_val_old = 0ull;
 595 +
 596 +       return hw->tf->write(hw->dev, ST_ASM330LHH_REG_TS2_ADDR, sizeof(data),
 597 +                            &data);
 598 +}
 599 +
 600 +int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw,
 601 +                              enum st_asm330lhh_fifo_mode fifo_mode)
 602 +{
 603 +       int err;
 604 +
 605 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_FIFO_CTRL4_ADDR,
 606 +                                          ST_ASM330LHH_REG_FIFO_MODE_MASK,
 607 +                                          fifo_mode);
 608 +       if (err < 0)
 609 +               return err;
 610 +
 611 +       hw->fifo_mode = fifo_mode;
 612 +
 613 +       return 0;
 614 +}
 615 +
 616 +static int st_asm330lhh_set_sensor_batching_odr(struct st_asm330lhh_sensor *sensor,
 617 +                                               bool enable)
 618 +{
 619 +       struct st_asm330lhh_hw *hw = sensor->hw;
 620 +       u8 data = 0;
 621 +       int err;
 622 +
 623 +       if (enable) {
 624 +               err = st_asm330lhh_get_odr_val(sensor->id, sensor->odr, &data);
 625 +               if (err < 0)
 626 +                       return err;
 627 +       }
 628 +
 629 +       return st_asm330lhh_write_with_mask(hw,
 630 +                                           sensor->batch_addr,
 631 +                                           sensor->batch_mask, data);
 632 +}
 633 +
 634 +static u16 st_asm330lhh_ts_odr(struct st_asm330lhh_hw *hw)
 635 +{
 636 +       struct st_asm330lhh_sensor *sensor;
 637 +       u16 odr = 0;
 638 +       u8 i;
 639 +
 640 +       for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
 641 +               if (!hw->iio_devs[i])
 642 +                       continue;
 643 +
 644 +               sensor = iio_priv(hw->iio_devs[i]);
 645 +               if (hw->enable_mask & BIT(sensor->id))
 646 +                       odr = max_t(u16, odr, sensor->odr);
 647 +       }
 648 +
 649 +       return odr;
 650 +}
 651 +
 652 +static int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,
 653 +                                        u16 watermark)
 654 +{
 655 +       u16 fifo_watermark = ST_ASM330LHH_MAX_FIFO_DEPTH, cur_watermark = 0;
 656 +       struct st_asm330lhh_hw *hw = sensor->hw;
 657 +       struct st_asm330lhh_sensor *cur_sensor;
 658 +       __le16 wdata;
 659 +       int i, err;
 660 +       u8 data;
 661 +
 662 +       for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
 663 +               cur_sensor = iio_priv(hw->iio_devs[i]);
 664 +
 665 +               if (!(hw->enable_mask & BIT(cur_sensor->id)))
 666 +                       continue;
 667 +
 668 +               cur_watermark = (cur_sensor == sensor) ? watermark
 669 +                                                      : cur_sensor->watermark;
 670 +
 671 +               fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
 672 +       }
 673 +
 674 +       fifo_watermark = max_t(u16, fifo_watermark, 2);
 675 +       mutex_lock(&hw->lock);
 676 +
 677 +       err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_FIFO_THL_ADDR + 1,
 678 +                          sizeof(data), &data);
 679 +       if (err < 0)
 680 +               goto out;
 681 +
 682 +       fifo_watermark = ((data << 8) & ~ST_ASM330LHH_REG_FIFO_LEN_MASK) |
 683 +                        (fifo_watermark & ST_ASM330LHH_REG_FIFO_LEN_MASK);
 684 +       wdata = cpu_to_le16(fifo_watermark);
 685 +       err = hw->tf->write(hw->dev, ST_ASM330LHH_REG_FIFO_THL_ADDR,
 686 +                           sizeof(wdata), (u8 *)&wdata);
 687 +
 688 +out:
 689 +       mutex_unlock(&hw->lock);
 690 +
 691 +       return err < 0 ? err : 0;
 692 +}
 693 +
 694 +static inline void st_asm330lhh_sync_hw_ts(struct st_asm330lhh_hw *hw, s64 ts)
 695 +{
 696 +       s64 delta = ts - hw->hw_ts;
 697 +
 698 +       hw->ts_offset = st_asm330lhh_ewma(hw->ts_offset, delta,
 699 +                                         ST_ASM330LHH_EWMA_LEVEL);
 700 +}
 701 +
 702 +static struct iio_dev *st_asm330lhh_get_iiodev_from_tag(struct st_asm330lhh_hw *hw,
 703 +                                                       u8 tag)
 704 +{
 705 +       struct iio_dev *iio_dev;
 706 +
 707 +       switch (tag) {
 708 +       case ST_ASM330LHH_GYRO_TAG:
 709 +               iio_dev = hw->iio_devs[ST_ASM330LHH_ID_GYRO];
 710 +               break;
 711 +       case ST_ASM330LHH_ACC_TAG:
 712 +               iio_dev = hw->iio_devs[ST_ASM330LHH_ID_ACC];
 713 +               break;
 714 +       default:
 715 +               iio_dev = NULL;
 716 +               break;
 717 +       }
 718 +
 719 +       return iio_dev;
 720 +}
 721 +
 722 +static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw)
 723 +{
 724 +       u8 iio_buf[ALIGN(ST_ASM330LHH_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
 725 +       u8 buf[6 * ST_ASM330LHH_FIFO_SAMPLE_SIZE], tag, *ptr;
 726 +       s64 ts_delta_hw_ts = 0, ts_irq;
 727 +       s64 ts_delta_offs;
 728 +       int i, err, read_len, word_len, fifo_len;
 729 +       struct st_asm330lhh_sensor *sensor;
 730 +       struct iio_dev *iio_dev;
 731 +       __le16 fifo_status;
 732 +       u16 fifo_depth;
 733 +       int ts_processed = 0;
 734 +       s64 hw_ts = 0ull, delta_hw_ts, cpu_timestamp;
 735 +
 736 +       ts_irq = hw->ts - hw->delta_ts;
 737 +
 738 +       do {
 739 +               err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_FIFO_DIFFL_ADDR,
 740 +                                  sizeof(fifo_status), (u8 *)&fifo_status);
 741 +               if (err < 0)
 742 +                       return err;
 743 +
 744 +               fifo_depth = le16_to_cpu(fifo_status) & ST_ASM330LHH_REG_FIFO_LEN_MASK;
 745 +               if (!fifo_depth)
 746 +                       return 0;
 747 +
 748 +               read_len = 0;
 749 +               fifo_len = fifo_depth * ST_ASM330LHH_FIFO_SAMPLE_SIZE;
 750 +               while (read_len < fifo_len) {
 751 +                       word_len = min_t(int, fifo_len - read_len, sizeof(buf));
 752 +                       err = hw->tf->read(hw->dev,
 753 +                                          ST_ASM330LHH_REG_FIFO_OUT_TAG_ADDR,
 754 +                                          word_len, buf);
 755 +                       if (err < 0)
 756 +                               return err;
 757 +
 758 +                       for (i = 0; i < word_len; i += ST_ASM330LHH_FIFO_SAMPLE_SIZE) {
 759 +                               ptr = &buf[i + ST_ASM330LHH_TAG_SIZE];
 760 +                               tag = buf[i] >> 3;
 761 +
 762 +                               if (tag == ST_ASM330LHH_TS_TAG) {
 763 +                                       hw->hw_val = get_unaligned_le32(ptr);
 764 +
 765 +                                       /* check for timer rollover */
 766 +                                       if (hw->hw_val < hw->hw_val_old) {
 767 +                                               hw->hw_ts_high++;
 768 +                                       }
 769 +
 770 +                                       hw->hw_ts = (hw->hw_val + (hw->hw_ts_high << 32)) * ST_ASM330LHH_TS_DELTA_NS;
 771 +                                       ts_delta_hw_ts = hw->hw_ts - hw->hw_ts_old;
 772 +                                       hw_ts += ts_delta_hw_ts;
 773 +                                       ts_delta_offs =
 774 +                                               div_s64(hw->delta_hw_ts * ST_ASM330LHH_MAX_ODR, hw->odr);
 775 +
 776 +                                       hw->ts_offset = st_asm330lhh_ewma(hw->ts_offset, ts_irq -
 777 +                                               hw->hw_ts + ts_delta_offs, ST_ASM330LHH_EWMA_LEVEL);
 778 +
 779 +                                       ts_irq += (hw->hw_ts + ts_delta_offs);
 780 +                                       hw->hw_ts_old = hw->hw_ts;
 781 +                                       hw->hw_val_old = hw->hw_val;
 782 +                                       ts_processed++;
 783 +
 784 +                                       if (!hw->tsample)
 785 +                                               hw->tsample =
 786 +                                                       hw->ts_offset + (hw->hw_ts + ts_delta_offs);
 787 +                                       else
 788 +                                               hw->tsample =
 789 +                                                       hw->tsample + (ts_delta_hw_ts + ts_delta_offs);
 790 +                               } else {
 791 +                                       iio_dev = st_asm330lhh_get_iiodev_from_tag(hw, tag);
 792 +                                       if (!iio_dev)
 793 +                                               continue;
 794 +
 795 +                                       sensor = iio_priv(iio_dev);
 796 +                                       if (sensor->std_samples < sensor->std_level) {
 797 +                                               sensor->std_samples++;
 798 +                                               continue;
 799 +                                       }
 800 +
 801 +                                       sensor = iio_priv(iio_dev);
 802 +
 803 +                                       /* Check if timestamp is in the future. */
 804 +                                       cpu_timestamp = st_asm330lhh_get_time_ns();
 805 +
 806 +                                       /* Avoid samples in the future. */
 807 +                                       if (hw->tsample > cpu_timestamp)
 808 +                                               hw->tsample = cpu_timestamp;
 809 +
 810 +                                       memcpy(iio_buf, ptr, ST_ASM330LHH_SAMPLE_SIZE);
 811 +                                       iio_push_to_buffers_with_timestamp(iio_dev,
 812 +                                                                          iio_buf,
 813 +                                                                          hw->tsample);
 814 +                               }
 815 +                       }
 816 +                       read_len += word_len;
 817 +               }
 818 +
 819 +               delta_hw_ts = div_s64(hw->delta_ts - hw_ts, ts_processed);
 820 +               delta_hw_ts = div_s64(delta_hw_ts * hw->odr, ST_ASM330LHH_MAX_ODR);
 821 +               hw->delta_hw_ts = st_asm330lhh_ewma(hw->delta_hw_ts,
 822 +                                                       delta_hw_ts,
 823 +                                                       ST_ASM330LHH_EWMA_LEVEL);
 824 +       } while(read_len);
 825 +
 826 +       return read_len;
 827 +}
 828 +
 829 +ssize_t st_asm330lhh_get_max_watermark(struct device *dev,
 830 +                                      struct device_attribute *attr, char *buf)
 831 +{
 832 +       return sprintf(buf, "%d\n", ST_ASM330LHH_MAX_FIFO_DEPTH);
 833 +}
 834 +
 835 +ssize_t st_asm330lhh_get_watermark(struct device *dev,
 836 +                                  struct device_attribute *attr, char *buf)
 837 +{
 838 +       struct iio_dev *iio_dev = dev_get_drvdata(dev);
 839 +       struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
 840 +
 841 +       return sprintf(buf, "%d\n", sensor->watermark);
 842 +}
 843 +
 844 +ssize_t st_asm330lhh_set_watermark(struct device *dev,
 845 +                                  struct device_attribute *attr,
 846 +                                  const char *buf, size_t size)
 847 +{
 848 +       struct iio_dev *iio_dev = dev_get_drvdata(dev);
 849 +       struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
 850 +       int err, val;
 851 +
 852 +       mutex_lock(&iio_dev->mlock);
 853 +       if (iio_buffer_enabled(iio_dev)) {
 854 +               err = -EBUSY;
 855 +               goto out;
 856 +       }
 857 +
 858 +       err = kstrtoint(buf, 10, &val);
 859 +       if (err < 0)
 860 +               goto out;
 861 +
 862 +       err = st_asm330lhh_update_watermark(sensor, val);
 863 +       if (err < 0)
 864 +               goto out;
 865 +
 866 +       sensor->watermark = val;
 867 +
 868 +out:
 869 +       mutex_unlock(&iio_dev->mlock);
 870 +
 871 +       return err < 0 ? err : size;
 872 +}
 873 +
 874 +ssize_t st_asm330lhh_flush_fifo(struct device *dev,
 875 +                               struct device_attribute *attr,
 876 +                               const char *buf, size_t size)
 877 +{
 878 +       struct iio_dev *iio_dev = dev_get_drvdata(dev);
 879 +       struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
 880 +       struct st_asm330lhh_hw *hw = sensor->hw;
 881 +       s64 type, event;
 882 +       int count;
 883 +       s64 ts;
 884 +
 885 +       mutex_lock(&hw->fifo_lock);
 886 +       ts = st_asm330lhh_get_time_ns();
 887 +       hw->delta_ts = ts - hw->ts;
 888 +       hw->ts = ts;
 889 +       set_bit(ST_ASM330LHH_HW_FLUSH, &hw->state);
 890 +
 891 +       count = st_asm330lhh_read_fifo(hw);
 892 +
 893 +       mutex_unlock(&hw->fifo_lock);
 894 +
 895 +       type = count > 0 ? CUSTOM_IIO_EV_DIR_FIFO_DATA : CUSTOM_IIO_EV_DIR_FIFO_EMPTY;
 896 +       event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1,
 897 +                                    CUSTOM_IIO_EV_TYPE_FIFO_FLUSH, type);
 898 +       iio_push_event(iio_dev, event, st_asm330lhh_get_time_ns());
 899 +
 900 +       return size;
 901 +}
 902 +
 903 +int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw)
 904 +{
 905 +       int err;
 906 +
 907 +       mutex_lock(&hw->fifo_lock);
 908 +
 909 +       st_asm330lhh_read_fifo(hw);
 910 +       err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_BYPASS);
 911 +
 912 +       mutex_unlock(&hw->fifo_lock);
 913 +
 914 +       return err;
 915 +}
 916 +
 917 +static int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable)
 918 +{
 919 +       struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
 920 +       struct st_asm330lhh_hw *hw = sensor->hw;
 921 +       int err;
 922 +
 923 +       mutex_lock(&hw->fifo_lock);
 924 +
 925 +       err = st_asm330lhh_sensor_set_enable(sensor, enable);
 926 +       if (err < 0)
 927 +               goto out;
 928 +
 929 +       err = st_asm330lhh_set_sensor_batching_odr(sensor, enable);
 930 +       if (err < 0)
 931 +               goto out;
 932 +
 933 +       err = st_asm330lhh_update_watermark(sensor, sensor->watermark);
 934 +       if (err < 0)
 935 +               goto out;
 936 +
 937 +       hw->odr = st_asm330lhh_ts_odr(hw);
 938 +
 939 +       if (enable && hw->fifo_mode == ST_ASM330LHH_FIFO_BYPASS) {
 940 +               st_asm330lhh_reset_hwts(hw);
 941 +               err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_CONT);
 942 +       } else if (!hw->enable_mask) {
 943 +               err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_BYPASS);
 944 +       }
 945 +
 946 +out:
 947 +       mutex_unlock(&hw->fifo_lock);
 948 +
 949 +       return err;
 950 +}
 951 +
 952 +static irqreturn_t st_asm330lhh_handler_irq(int irq, void *private)
 953 +{
 954 +       struct st_asm330lhh_hw *hw = (struct st_asm330lhh_hw *)private;
 955 +       s64 ts = st_asm330lhh_get_time_ns();
 956 +
 957 +       hw->delta_ts = ts - hw->ts;
 958 +       hw->ts = ts;
 959 +
 960 +       return IRQ_WAKE_THREAD;
 961 +}
 962 +
 963 +static irqreturn_t st_asm330lhh_handler_thread(int irq, void *private)
 964 +{
 965 +       struct st_asm330lhh_hw *hw = (struct st_asm330lhh_hw *)private;
 966 +
 967 +       mutex_lock(&hw->fifo_lock);
 968 +
 969 +       st_asm330lhh_read_fifo(hw);
 970 +       clear_bit(ST_ASM330LHH_HW_FLUSH, &hw->state);
 971 +
 972 +       mutex_unlock(&hw->fifo_lock);
 973 +
 974 +       return IRQ_HANDLED;
 975 +}
 976 +
 977 +static int st_asm330lhh_buffer_preenable(struct iio_dev *iio_dev)
 978 +{
 979 +       return st_asm330lhh_update_fifo(iio_dev, true);
 980 +}
 981 +
 982 +static int st_asm330lhh_buffer_postdisable(struct iio_dev *iio_dev)
 983 +{
 984 +       return st_asm330lhh_update_fifo(iio_dev, false);
 985 +}
 986 +
 987 +static const struct iio_buffer_setup_ops st_asm330lhh_buffer_ops = {
 988 +       .preenable = st_asm330lhh_buffer_preenable,
 989 +       .postdisable = st_asm330lhh_buffer_postdisable,
 990 +};
 991 +
 992 +static int st_asm330lhh_fifo_init(struct st_asm330lhh_hw *hw)
 993 +{
 994 +       return st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_FIFO_CTRL4_ADDR,
 995 +                                           ST_ASM330LHH_REG_DEC_TS_MASK, 1);
 996 +}
 997 +
 998 +int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw)
 999 +{
1000 +       struct device_node *np = hw->dev->of_node;
1001 +       struct iio_buffer *buffer;
1002 +       unsigned long irq_type;
1003 +       bool irq_active_low;
1004 +       int i, err;
1005 +
1006 +       irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
1007 +
1008 +       switch (irq_type) {
1009 +       case IRQF_TRIGGER_HIGH:
1010 +       case IRQF_TRIGGER_RISING:
1011 +               irq_active_low = false;
1012 +               break;
1013 +       case IRQF_TRIGGER_LOW:
1014 +       case IRQF_TRIGGER_FALLING:
1015 +               irq_active_low = true;
1016 +               break;
1017 +       default:
1018 +               dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
1019 +               return -EINVAL;
1020 +       }
1021 +
1022 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_HLACTIVE_ADDR,
1023 +                                          ST_ASM330LHH_REG_HLACTIVE_MASK,
1024 +                                          irq_active_low);
1025 +       if (err < 0)
1026 +               return err;
1027 +
1028 +       if (np && of_property_read_bool(np, "drive-open-drain")) {
1029 +               err = st_asm330lhh_write_with_mask(hw,
1030 +                                       ST_ASM330LHH_REG_PP_OD_ADDR,
1031 +                                       ST_ASM330LHH_REG_PP_OD_MASK, 1);
1032 +               if (err < 0)
1033 +                       return err;
1034 +
1035 +               irq_type |= IRQF_SHARED;
1036 +       }
1037 +
1038 +       err = devm_request_threaded_irq(hw->dev, hw->irq,
1039 +                                       st_asm330lhh_handler_irq,
1040 +                                       st_asm330lhh_handler_thread,
1041 +                                       irq_type | IRQF_ONESHOT,
1042 +                                       "asm330lhh", hw);
1043 +       if (err) {
1044 +               dev_err(hw->dev, "failed to request trigger irq %d\n",
1045 +                       hw->irq);
1046 +               return err;
1047 +       }
1048 +
1049 +       for (i = ST_ASM330LHH_ID_ACC; i < ST_ASM330LHH_ID_MAX; i++) {
1050 +               if (!hw->iio_devs[i])
1051 +                       continue;
1052 +
1053 +               buffer = devm_iio_kfifo_allocate(hw->dev);
1054 +               if (!buffer)
1055 +                       return -ENOMEM;
1056 +
1057 +               iio_device_attach_buffer(hw->iio_devs[i], buffer);
1058 +               hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
1059 +               hw->iio_devs[i]->setup_ops = &st_asm330lhh_buffer_ops;
1060 +       }
1061 +
1062 +       return st_asm330lhh_fifo_init(hw);
1063 +}
1064 +
1065 diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
1066 new file mode 100644
1067 index 0000000..9d9ee20
1068 --- /dev/null
1069 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
1070 @@ -0,0 +1,824 @@
1071 +/*
1072 + * STMicroelectronics st_asm330lhh sensor driver
1073 + *
1074 + * Copyright 2018 STMicroelectronics Inc.
1075 + *
1076 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
1077 + *
1078 + * Licensed under the GPL-2.
1079 + */
1080 +
1081 +#include <linux/kernel.h>
1082 +#include <linux/module.h>
1083 +#include <linux/delay.h>
1084 +#include <linux/iio/iio.h>
1085 +#include <linux/iio/sysfs.h>
1086 +#include <linux/pm.h>
1087 +#include <linux/version.h>
1088 +#include <linux/of.h>
1089 +
1090 +#include <linux/platform_data/st_sensors_pdata.h>
1091 +
1092 +#include "st_asm330lhh.h"
1093 +
1094 +#define ST_ASM330LHH_REG_INT1_ADDR             0x0d
1095 +#define ST_ASM330LHH_REG_INT2_ADDR             0x0e
1096 +#define ST_ASM330LHH_REG_FIFO_CTRL4_ADDR       0x0a
1097 +#define ST_ASM330LHH_REG_FIFO_FTH_IRQ_MASK     BIT(3)
1098 +#define ST_ASM330LHH_REG_WHOAMI_ADDR           0x0f
1099 +#define ST_ASM330LHH_WHOAMI_VAL                        0x6b
1100 +#define ST_ASM330LHH_REG_CTRL1_XL_ADDR         0x10
1101 +#define ST_ASM330LHH_REG_CTRL2_G_ADDR          0x11
1102 +#define ST_ASM330LHH_REG_RESET_ADDR            0x12
1103 +#define ST_ASM330LHH_REG_RESET_MASK            BIT(0)
1104 +#define ST_ASM330LHH_REG_BDU_ADDR              0x12
1105 +#define ST_ASM330LHH_REG_BDU_MASK              BIT(6)
1106 +#define ST_ASM330LHH_REG_INT2_ON_INT1_ADDR     0x13
1107 +#define ST_ASM330LHH_REG_INT2_ON_INT1_MASK     BIT(5)
1108 +#define ST_ASM330LHH_REG_ROUNDING_ADDR         0x14
1109 +#define ST_ASM330LHH_REG_ROUNDING_MASK         GENMASK(6, 5)
1110 +#define ST_ASM330LHH_REG_TIMESTAMP_EN_ADDR     0x19
1111 +#define ST_ASM330LHH_REG_TIMESTAMP_EN_MASK     BIT(5)
1112 +
1113 +#define ST_ASM330LHH_REG_GYRO_OUT_X_L_ADDR     0x22
1114 +#define ST_ASM330LHH_REG_GYRO_OUT_Y_L_ADDR     0x24
1115 +#define ST_ASM330LHH_REG_GYRO_OUT_Z_L_ADDR     0x26
1116 +
1117 +#define ST_ASM330LHH_REG_ACC_OUT_X_L_ADDR      0x28
1118 +#define ST_ASM330LHH_REG_ACC_OUT_Y_L_ADDR      0x2a
1119 +#define ST_ASM330LHH_REG_ACC_OUT_Z_L_ADDR      0x2c
1120 +
1121 +#define ST_ASM330LHH_REG_LIR_ADDR              0x56
1122 +#define ST_ASM330LHH_REG_LIR_MASK              BIT(0)
1123 +
1124 +#define ST_ASM330LHH_ACC_FS_2G_GAIN            IIO_G_TO_M_S_2(61)
1125 +#define ST_ASM330LHH_ACC_FS_4G_GAIN            IIO_G_TO_M_S_2(122)
1126 +#define ST_ASM330LHH_ACC_FS_8G_GAIN            IIO_G_TO_M_S_2(244)
1127 +#define ST_ASM330LHH_ACC_FS_16G_GAIN           IIO_G_TO_M_S_2(488)
1128 +
1129 +#define ST_ASM330LHH_GYRO_FS_125_GAIN          IIO_DEGREE_TO_RAD(4375)
1130 +#define ST_ASM330LHH_GYRO_FS_250_GAIN          IIO_DEGREE_TO_RAD(8750)
1131 +#define ST_ASM330LHH_GYRO_FS_500_GAIN          IIO_DEGREE_TO_RAD(17500)
1132 +#define ST_ASM330LHH_GYRO_FS_1000_GAIN         IIO_DEGREE_TO_RAD(35000)
1133 +#define ST_ASM330LHH_GYRO_FS_2000_GAIN         IIO_DEGREE_TO_RAD(70000)
1134 +#define ST_ASM330LHH_GYRO_FS_4000_GAIN         IIO_DEGREE_TO_RAD(140000)
1135 +
1136 +/* Temperature in uC */
1137 +#define ST_ASM330LHH_TEMP_GAIN                 256
1138 +#define ST_ASM330LHH_TEMP_FS_GAIN              (1000000 / ST_ASM330LHH_TEMP_GAIN)
1139 +#define ST_ASM330LHH_OFFSET                    (6400)
1140 +
1141 +struct st_asm330lhh_std_entry {
1142 +       u16 odr;
1143 +       u8 val;
1144 +};
1145 +
1146 +/* Minimal number of sample to be discarded */
1147 +struct st_asm330lhh_std_entry st_asm330lhh_std_table[] = {
1148 +       {  13,  2 },
1149 +       {  26,  3 },
1150 +       {  52,  4 },
1151 +       { 104,  6 },
1152 +       { 208,  8 },
1153 +       { 416, 18 },
1154 +};
1155 +
1156 +static const struct st_asm330lhh_odr_table_entry st_asm330lhh_odr_table[] = {
1157 +       [ST_ASM330LHH_ID_ACC] = {
1158 +               .reg = {
1159 +                       .addr = ST_ASM330LHH_REG_CTRL1_XL_ADDR,
1160 +                       .mask = GENMASK(7, 4),
1161 +               },
1162 +               .odr_avl[0] = {   0, 0x00 },
1163 +               .odr_avl[1] = {  13, 0x01 },
1164 +               .odr_avl[2] = {  26, 0x02 },
1165 +               .odr_avl[3] = {  52, 0x03 },
1166 +               .odr_avl[4] = { 104, 0x04 },
1167 +               .odr_avl[5] = { 208, 0x05 },
1168 +               .odr_avl[6] = { 416, 0x06 },
1169 +       },
1170 +       [ST_ASM330LHH_ID_GYRO] = {
1171 +               .reg = {
1172 +                       .addr = ST_ASM330LHH_REG_CTRL2_G_ADDR,
1173 +                       .mask = GENMASK(7, 4),
1174 +               },
1175 +               .odr_avl[0] = {   0, 0x00 },
1176 +               .odr_avl[1] = {  13, 0x01 },
1177 +               .odr_avl[2] = {  26, 0x02 },
1178 +               .odr_avl[3] = {  52, 0x03 },
1179 +               .odr_avl[4] = { 104, 0x04 },
1180 +               .odr_avl[5] = { 208, 0x05 },
1181 +               .odr_avl[6] = { 416, 0x06 },
1182 +       },
1183 +       [ST_ASM330LHH_ID_TEMP] = {
1184 +               .odr_avl[0] = {   0, 0x00 },
1185 +               .odr_avl[1] = {  52, 0x01 },
1186 +       }
1187 +};
1188 +
1189 +static const struct st_asm330lhh_fs_table_entry st_asm330lhh_fs_table[] = {
1190 +       [ST_ASM330LHH_ID_ACC] = {
1191 +               .reg = {
1192 +                       .addr = ST_ASM330LHH_REG_CTRL1_XL_ADDR,
1193 +                       .mask = GENMASK(3, 2),
1194 +               },
1195 +               .size = ST_ASM330LHH_FS_ACC_LIST_SIZE,
1196 +               .fs_avl[0] = {  ST_ASM330LHH_ACC_FS_2G_GAIN, 0x0 },
1197 +               .fs_avl[1] = {  ST_ASM330LHH_ACC_FS_4G_GAIN, 0x2 },
1198 +               .fs_avl[2] = {  ST_ASM330LHH_ACC_FS_8G_GAIN, 0x3 },
1199 +               .fs_avl[3] = { ST_ASM330LHH_ACC_FS_16G_GAIN, 0x1 },
1200 +       },
1201 +       [ST_ASM330LHH_ID_GYRO] = {
1202 +               .reg = {
1203 +                       .addr = ST_ASM330LHH_REG_CTRL2_G_ADDR,
1204 +                       .mask = GENMASK(3, 0),
1205 +               },
1206 +               .size = ST_ASM330LHH_FS_GYRO_LIST_SIZE,
1207 +               .fs_avl[0] = {  ST_ASM330LHH_GYRO_FS_125_GAIN, 0x2 },
1208 +               .fs_avl[1] = {  ST_ASM330LHH_GYRO_FS_250_GAIN, 0x0 },
1209 +               .fs_avl[2] = {  ST_ASM330LHH_GYRO_FS_500_GAIN, 0x4 },
1210 +               .fs_avl[3] = { ST_ASM330LHH_GYRO_FS_1000_GAIN, 0x8 },
1211 +               .fs_avl[4] = { ST_ASM330LHH_GYRO_FS_2000_GAIN, 0xC },
1212 +               .fs_avl[5] = { ST_ASM330LHH_GYRO_FS_4000_GAIN, 0x1 },
1213 +       },
1214 +       [ST_ASM330LHH_ID_TEMP] = {
1215 +               .size = ST_ASM330LHH_FS_TEMP_LIST_SIZE,
1216 +               .fs_avl[0] = {  ST_ASM330LHH_TEMP_FS_GAIN, 0x0 },
1217 +       }
1218 +};
1219 +
1220 +static const struct iio_chan_spec st_asm330lhh_acc_channels[] = {
1221 +       ST_ASM330LHH_CHANNEL(IIO_ACCEL, ST_ASM330LHH_REG_ACC_OUT_X_L_ADDR,
1222 +                          1, IIO_MOD_X, 0, 16, 16, 's'),
1223 +       ST_ASM330LHH_CHANNEL(IIO_ACCEL, ST_ASM330LHH_REG_ACC_OUT_Y_L_ADDR,
1224 +                          1, IIO_MOD_Y, 1, 16, 16, 's'),
1225 +       ST_ASM330LHH_CHANNEL(IIO_ACCEL, ST_ASM330LHH_REG_ACC_OUT_Z_L_ADDR,
1226 +                          1, IIO_MOD_Z, 2, 16, 16, 's'),
1227 +       ST_ASM330LHH_FLUSH_CHANNEL(IIO_ACCEL),
1228 +       IIO_CHAN_SOFT_TIMESTAMP(3),
1229 +};
1230 +
1231 +static const struct iio_chan_spec st_asm330lhh_gyro_channels[] = {
1232 +       ST_ASM330LHH_CHANNEL(IIO_ANGL_VEL, ST_ASM330LHH_REG_GYRO_OUT_X_L_ADDR,
1233 +                          1, IIO_MOD_X, 0, 16, 16, 's'),
1234 +       ST_ASM330LHH_CHANNEL(IIO_ANGL_VEL, ST_ASM330LHH_REG_GYRO_OUT_Y_L_ADDR,
1235 +                          1, IIO_MOD_Y, 1, 16, 16, 's'),
1236 +       ST_ASM330LHH_CHANNEL(IIO_ANGL_VEL, ST_ASM330LHH_REG_GYRO_OUT_Z_L_ADDR,
1237 +                          1, IIO_MOD_Z, 2, 16, 16, 's'),
1238 +       ST_ASM330LHH_FLUSH_CHANNEL(IIO_ANGL_VEL),
1239 +       IIO_CHAN_SOFT_TIMESTAMP(3),
1240 +};
1241 +
1242 +static const struct iio_chan_spec st_asm330lhh_temp_channels[] = {
1243 +       {
1244 +               .type = IIO_TEMP,
1245 +               .address = ST_ASM330LHH_REG_OUT_TEMP_L_ADDR,
1246 +               .info_mask_separate = BIT(IIO_CHAN_INFO_RAW)
1247 +                               | BIT(IIO_CHAN_INFO_OFFSET)
1248 +                               | BIT(IIO_CHAN_INFO_SCALE),
1249 +               .scan_index = -1,
1250 +       },
1251 +};
1252 +
1253 +int st_asm330lhh_write_with_mask(struct st_asm330lhh_hw *hw, u8 addr, u8 mask,
1254 +                                u8 val)
1255 +{
1256 +       u8 data;
1257 +       int err;
1258 +
1259 +       mutex_lock(&hw->lock);
1260 +
1261 +       err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
1262 +       if (err < 0) {
1263 +               dev_err(hw->dev, "failed to read %02x register\n", addr);
1264 +               goto out;
1265 +       }
1266 +
1267 +       data = (data & ~mask) | ((val << __ffs(mask)) & mask);
1268 +
1269 +       err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
1270 +       if (err < 0)
1271 +               dev_err(hw->dev, "failed to write %02x register\n", addr);
1272 +
1273 +out:
1274 +       mutex_unlock(&hw->lock);
1275 +
1276 +       return err;
1277 +}
1278 +
1279 +static int st_asm330lhh_check_whoami(struct st_asm330lhh_hw *hw)
1280 +{
1281 +       int err;
1282 +       u8 data;
1283 +
1284 +       err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_WHOAMI_ADDR, sizeof(data),
1285 +                          &data);
1286 +       if (err < 0) {
1287 +               dev_err(hw->dev, "failed to read whoami register\n");
1288 +               return err;
1289 +       }
1290 +
1291 +       if (data != ST_ASM330LHH_WHOAMI_VAL) {
1292 +               dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
1293 +               return -ENODEV;
1294 +       }
1295 +
1296 +       return 0;
1297 +}
1298 +
1299 +static int st_asm330lhh_set_full_scale(struct st_asm330lhh_sensor *sensor,
1300 +                                      u32 gain)
1301 +{
1302 +       enum st_asm330lhh_sensor_id id = sensor->id;
1303 +       int i, err;
1304 +       u8 val;
1305 +
1306 +       for (i = 0; i < st_asm330lhh_fs_table[id].size; i++)
1307 +               if (st_asm330lhh_fs_table[id].fs_avl[i].gain == gain)
1308 +                       break;
1309 +
1310 +       if (i == st_asm330lhh_fs_table[id].size)
1311 +               return -EINVAL;
1312 +
1313 +       val = st_asm330lhh_fs_table[id].fs_avl[i].val;
1314 +       err = st_asm330lhh_write_with_mask(sensor->hw,
1315 +                                       st_asm330lhh_fs_table[id].reg.addr,
1316 +                                       st_asm330lhh_fs_table[id].reg.mask,
1317 +                                       val);
1318 +       if (err < 0)
1319 +               return err;
1320 +
1321 +       sensor->gain = gain;
1322 +
1323 +       return 0;
1324 +}
1325 +
1326 +int st_asm330lhh_get_odr_val(enum st_asm330lhh_sensor_id id, u16 odr, u8 *val)
1327 +{
1328 +       int i;
1329 +
1330 +       for (i = 0; i < ST_ASM330LHH_ODR_LIST_SIZE; i++)
1331 +               if (st_asm330lhh_odr_table[id].odr_avl[i].hz >= odr)
1332 +                       break;
1333 +
1334 +       if (i == ST_ASM330LHH_ODR_LIST_SIZE)
1335 +               return -EINVAL;
1336 +
1337 +       *val = st_asm330lhh_odr_table[id].odr_avl[i].val;
1338 +
1339 +       return 0;
1340 +}
1341 +
1342 +static int st_asm330lhh_set_std_level(struct st_asm330lhh_sensor *sensor,
1343 +                       u16 odr)
1344 +{
1345 +       int i;
1346 +
1347 +       for (i = 0; i < ARRAY_SIZE(st_asm330lhh_std_table); i++)
1348 +               if (st_asm330lhh_std_table[i].odr == odr)
1349 +                       break;
1350 +
1351 +       if (i == ARRAY_SIZE(st_asm330lhh_std_table))
1352 +               return -EINVAL;
1353 +
1354 +       sensor->std_level = st_asm330lhh_std_table[i].val;
1355 +       sensor->std_samples = 0;
1356 +
1357 +       return 0;
1358 +}
1359 +
1360 +static int st_asm330lhh_set_odr(struct st_asm330lhh_sensor *sensor, u16 odr)
1361 +{
1362 +       struct st_asm330lhh_hw *hw = sensor->hw;
1363 +       u8 val;
1364 +
1365 +       if (st_asm330lhh_get_odr_val(sensor->id, odr, &val) < 0)
1366 +               return -EINVAL;
1367 +
1368 +       return st_asm330lhh_write_with_mask(hw,
1369 +                               st_asm330lhh_odr_table[sensor->id].reg.addr,
1370 +                               st_asm330lhh_odr_table[sensor->id].reg.mask, val);
1371 +}
1372 +
1373 +int st_asm330lhh_sensor_set_enable(struct st_asm330lhh_sensor *sensor,
1374 +                                  bool enable)
1375 +{
1376 +       u16 odr = enable ? sensor->odr : 0;
1377 +       int err;
1378 +
1379 +       if (sensor->id != ST_ASM330LHH_ID_TEMP) {
1380 +               err = st_asm330lhh_set_odr(sensor, odr);
1381 +               if (err < 0)
1382 +                       return err;
1383 +       }
1384 +
1385 +       if (enable)
1386 +               sensor->hw->enable_mask |= BIT(sensor->id);
1387 +       else
1388 +               sensor->hw->enable_mask &= ~BIT(sensor->id);
1389 +
1390 +       return 0;
1391 +}
1392 +
1393 +static int st_asm330lhh_read_oneshot(struct st_asm330lhh_sensor *sensor,
1394 +                                    u8 addr, int *val)
1395 +{
1396 +       int err, delay;
1397 +       __le16 data;
1398 +
1399 +       if (sensor->id == ST_ASM330LHH_ID_TEMP) {
1400 +               u8 status;
1401 +
1402 +               mutex_lock(&sensor->hw->fifo_lock);
1403 +               err = sensor->hw->tf->read(sensor->hw->dev,
1404 +                                          ST_ASM330LHH_REG_STATUS_ADDR, sizeof(status), &status);
1405 +               if (err < 0)
1406 +                       goto unlock;
1407 +
1408 +               if (status & ST_ASM330LHH_REG_STATUS_TDA) {
1409 +                       err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
1410 +                                          (u8 *)&data);
1411 +                       if (err < 0)
1412 +                               goto unlock;
1413 +
1414 +                       sensor->old_data = data;
1415 +               } else
1416 +                       data = sensor->old_data;
1417 +unlock:
1418 +               mutex_unlock(&sensor->hw->fifo_lock);
1419 +
1420 +       } else {
1421 +               err = st_asm330lhh_sensor_set_enable(sensor, true);
1422 +               if (err < 0)
1423 +                       return err;
1424 +
1425 +               delay = 1000000 / sensor->odr;
1426 +               usleep_range(delay, 2 * delay);
1427 +
1428 +               err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
1429 +                                          (u8 *)&data);
1430 +               if (err < 0)
1431 +                       return err;
1432 +
1433 +               st_asm330lhh_sensor_set_enable(sensor, false);
1434 +       }
1435 +
1436 +       *val = (s16)data;
1437 +
1438 +       return IIO_VAL_INT;
1439 +}
1440 +
1441 +static int st_asm330lhh_read_raw(struct iio_dev *iio_dev,
1442 +                                struct iio_chan_spec const *ch,
1443 +                                int *val, int *val2, long mask)
1444 +{
1445 +       struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
1446 +       int ret;
1447 +
1448 +       switch (mask) {
1449 +       case IIO_CHAN_INFO_RAW:
1450 +               mutex_lock(&iio_dev->mlock);
1451 +               if (iio_buffer_enabled(iio_dev)) {
1452 +                       ret = -EBUSY;
1453 +                       mutex_unlock(&iio_dev->mlock);
1454 +                       break;
1455 +               }
1456 +               ret = st_asm330lhh_read_oneshot(sensor, ch->address, val);
1457 +               mutex_unlock(&iio_dev->mlock);
1458 +               break;
1459 +       case IIO_CHAN_INFO_OFFSET:
1460 +               switch (ch->type) {
1461 +               case IIO_TEMP:
1462 +                       *val = sensor->offset;
1463 +                       ret = IIO_VAL_INT;
1464 +                       break;
1465 +               default:
1466 +                       return -EINVAL;
1467 +               }
1468 +               break;
1469 +       case IIO_CHAN_INFO_SAMP_FREQ:
1470 +               *val = sensor->odr;
1471 +               ret = IIO_VAL_INT;
1472 +               break;
1473 +       case IIO_CHAN_INFO_SCALE:
1474 +               switch (ch->type) {
1475 +               case IIO_TEMP:
1476 +                       *val = 1;
1477 +                       *val2 = ST_ASM330LHH_TEMP_GAIN;
1478 +                       ret = IIO_VAL_FRACTIONAL;
1479 +                       break;
1480 +               case IIO_ACCEL:
1481 +               case IIO_ANGL_VEL:
1482 +                       *val = 0;
1483 +                       *val2 = sensor->gain;
1484 +                       ret = IIO_VAL_INT_PLUS_MICRO;
1485 +                       break;
1486 +               default:
1487 +                       return -EINVAL;
1488 +               }
1489 +               break;
1490 +       default:
1491 +               ret = -EINVAL;
1492 +               break;
1493 +       }
1494 +
1495 +       return ret;
1496 +}
1497 +
1498 +static int st_asm330lhh_write_raw(struct iio_dev *iio_dev,
1499 +                                 struct iio_chan_spec const *chan,
1500 +                                 int val, int val2, long mask)
1501 +{
1502 +       struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
1503 +       int err;
1504 +
1505 +       mutex_lock(&iio_dev->mlock);
1506 +
1507 +       switch (mask) {
1508 +       case IIO_CHAN_INFO_SCALE:
1509 +               err = st_asm330lhh_set_full_scale(sensor, val2);
1510 +               break;
1511 +       case IIO_CHAN_INFO_SAMP_FREQ: {
1512 +               u8 data;
1513 +
1514 +               err = st_asm330lhh_set_std_level(sensor, val);
1515 +               if (err < 0)
1516 +                       break;
1517 +
1518 +               err = st_asm330lhh_get_odr_val(sensor->id, val, &data);
1519 +               if (!err)
1520 +                       sensor->odr = val;
1521 +
1522 +               err = st_asm330lhh_set_odr(sensor, sensor->odr);
1523 +               break;
1524 +       }
1525 +       default:
1526 +               err = -EINVAL;
1527 +               break;
1528 +       }
1529 +
1530 +       mutex_unlock(&iio_dev->mlock);
1531 +
1532 +       return err;
1533 +}
1534 +
1535 +static ssize_t
1536 +st_asm330lhh_sysfs_sampling_frequency_avail(struct device *dev,
1537 +                                           struct device_attribute *attr,
1538 +                                           char *buf)
1539 +{
1540 +       struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));
1541 +       enum st_asm330lhh_sensor_id id = sensor->id;
1542 +       int i, len = 0;
1543 +
1544 +       for (i = 1; i < ST_ASM330LHH_ODR_LIST_SIZE; i++)
1545 +               len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
1546 +                                st_asm330lhh_odr_table[id].odr_avl[i].hz);
1547 +       buf[len - 1] = '\n';
1548 +
1549 +       return len;
1550 +}
1551 +
1552 +static ssize_t st_asm330lhh_sysfs_scale_avail(struct device *dev,
1553 +                                             struct device_attribute *attr,
1554 +                                             char *buf)
1555 +{
1556 +       struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));
1557 +       enum st_asm330lhh_sensor_id id = sensor->id;
1558 +       int i, len = 0;
1559 +
1560 +       for (i = 0; i < st_asm330lhh_fs_table[id].size; i++)
1561 +               len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
1562 +                                st_asm330lhh_fs_table[id].fs_avl[i].gain);
1563 +       buf[len - 1] = '\n';
1564 +
1565 +       return len;
1566 +}
1567 +
1568 +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_asm330lhh_sysfs_sampling_frequency_avail);
1569 +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
1570 +                      st_asm330lhh_sysfs_scale_avail, NULL, 0);
1571 +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
1572 +                      st_asm330lhh_sysfs_scale_avail, NULL, 0);
1573 +static IIO_DEVICE_ATTR(in_temp_scale_available, 0444,
1574 +                      st_asm330lhh_sysfs_scale_avail, NULL, 0);
1575 +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444,
1576 +                      st_asm330lhh_get_max_watermark, NULL, 0);
1577 +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_asm330lhh_flush_fifo, 0);
1578 +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_asm330lhh_get_watermark,
1579 +                      st_asm330lhh_set_watermark, 0);
1580 +
1581 +static struct attribute *st_asm330lhh_acc_attributes[] = {
1582 +       &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
1583 +       &iio_dev_attr_in_accel_scale_available.dev_attr.attr,
1584 +       &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1585 +       &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1586 +       &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1587 +       NULL,
1588 +};
1589 +
1590 +static const struct attribute_group st_asm330lhh_acc_attribute_group = {
1591 +       .attrs = st_asm330lhh_acc_attributes,
1592 +};
1593 +
1594 +static const struct iio_info st_asm330lhh_acc_info = {
1595 +       .driver_module = THIS_MODULE,
1596 +       .attrs = &st_asm330lhh_acc_attribute_group,
1597 +       .read_raw = st_asm330lhh_read_raw,
1598 +       .write_raw = st_asm330lhh_write_raw,
1599 +};
1600 +
1601 +static struct attribute *st_asm330lhh_gyro_attributes[] = {
1602 +       &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
1603 +       &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
1604 +       &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1605 +       &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1606 +       &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1607 +       NULL,
1608 +};
1609 +
1610 +static const struct attribute_group st_asm330lhh_gyro_attribute_group = {
1611 +       .attrs = st_asm330lhh_gyro_attributes,
1612 +};
1613 +
1614 +static const struct iio_info st_asm330lhh_gyro_info = {
1615 +       .driver_module = THIS_MODULE,
1616 +       .attrs = &st_asm330lhh_gyro_attribute_group,
1617 +       .read_raw = st_asm330lhh_read_raw,
1618 +       .write_raw = st_asm330lhh_write_raw,
1619 +};
1620 +
1621 +static struct attribute *st_asm330lhh_temp_attributes[] = {
1622 +       &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
1623 +       &iio_dev_attr_in_temp_scale_available.dev_attr.attr,
1624 +       &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1625 +       &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1626 +       &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1627 +       NULL,
1628 +};
1629 +
1630 +static const struct attribute_group st_asm330lhh_temp_attribute_group = {
1631 +       .attrs = st_asm330lhh_temp_attributes,
1632 +};
1633 +
1634 +static const struct iio_info st_asm330lhh_temp_info = {
1635 +       .driver_module = THIS_MODULE,
1636 +       .attrs = &st_asm330lhh_temp_attribute_group,
1637 +       .read_raw = st_asm330lhh_read_raw,
1638 +       .write_raw = st_asm330lhh_write_raw,
1639 +};
1640 +
1641 +static const unsigned long st_asm330lhh_available_scan_masks[] = { 0x7, 0x0 };
1642 +
1643 +static int st_asm330lhh_of_get_drdy_pin(struct st_asm330lhh_hw *hw, int *drdy_pin)
1644 +{
1645 +       struct device_node *np = hw->dev->of_node;
1646 +
1647 +       if (!np)
1648 +               return -EINVAL;
1649 +
1650 +       return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
1651 +}
1652 +
1653 +static int st_asm330lhh_get_drdy_reg(struct st_asm330lhh_hw *hw, u8 *drdy_reg)
1654 +{
1655 +       int err = 0, drdy_pin;
1656 +
1657 +       if (st_asm330lhh_of_get_drdy_pin(hw, &drdy_pin) < 0) {
1658 +               struct st_sensors_platform_data *pdata;
1659 +               struct device *dev = hw->dev;
1660 +
1661 +               pdata = (struct st_sensors_platform_data *)dev->platform_data;
1662 +               drdy_pin = pdata ? pdata->drdy_int_pin : 1;
1663 +       }
1664 +
1665 +       switch (drdy_pin) {
1666 +       case 1:
1667 +               *drdy_reg = ST_ASM330LHH_REG_INT1_ADDR;
1668 +               break;
1669 +       case 2:
1670 +               *drdy_reg = ST_ASM330LHH_REG_INT2_ADDR;
1671 +               break;
1672 +       default:
1673 +               dev_err(hw->dev, "unsupported data ready pin\n");
1674 +               err = -EINVAL;
1675 +               break;
1676 +       }
1677 +
1678 +       return err;
1679 +}
1680 +
1681 +static int st_asm330lhh_init_device(struct st_asm330lhh_hw *hw)
1682 +{
1683 +       u8 drdy_int_reg;
1684 +       int err;
1685 +
1686 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_RESET_ADDR,
1687 +                                          ST_ASM330LHH_REG_RESET_MASK, 1);
1688 +       if (err < 0)
1689 +               return err;
1690 +
1691 +       msleep(200);
1692 +
1693 +       /* latch interrupts */
1694 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_LIR_ADDR,
1695 +                                          ST_ASM330LHH_REG_LIR_MASK, 1);
1696 +       if (err < 0)
1697 +               return err;
1698 +
1699 +       /* enable Block Data Update */
1700 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_BDU_ADDR,
1701 +                                          ST_ASM330LHH_REG_BDU_MASK, 1);
1702 +       if (err < 0)
1703 +               return err;
1704 +
1705 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_ROUNDING_ADDR,
1706 +                                          ST_ASM330LHH_REG_ROUNDING_MASK, 3);
1707 +       if (err < 0)
1708 +               return err;
1709 +
1710 +       /* init timestamp engine */
1711 +       err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_TIMESTAMP_EN_ADDR,
1712 +                                          ST_ASM330LHH_REG_TIMESTAMP_EN_MASK, 1);
1713 +       if (err < 0)
1714 +               return err;
1715 +
1716 +       /* enable FIFO watermak interrupt */
1717 +       err = st_asm330lhh_get_drdy_reg(hw, &drdy_int_reg);
1718 +       if (err < 0)
1719 +               return err;
1720 +
1721 +       return st_asm330lhh_write_with_mask(hw, drdy_int_reg,
1722 +                                           ST_ASM330LHH_REG_FIFO_FTH_IRQ_MASK, 1);
1723 +}
1724 +
1725 +static struct iio_dev *st_asm330lhh_alloc_iiodev(struct st_asm330lhh_hw *hw,
1726 +                                                enum st_asm330lhh_sensor_id id)
1727 +{
1728 +       struct st_asm330lhh_sensor *sensor;
1729 +       struct iio_dev *iio_dev;
1730 +
1731 +       iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
1732 +       if (!iio_dev)
1733 +               return NULL;
1734 +
1735 +       iio_dev->modes = INDIO_DIRECT_MODE;
1736 +       iio_dev->dev.parent = hw->dev;
1737 +       iio_dev->available_scan_masks = st_asm330lhh_available_scan_masks;
1738 +
1739 +       sensor = iio_priv(iio_dev);
1740 +       sensor->id = id;
1741 +       sensor->hw = hw;
1742 +       sensor->odr = st_asm330lhh_odr_table[id].odr_avl[1].hz;
1743 +       sensor->gain = st_asm330lhh_fs_table[id].fs_avl[0].gain;
1744 +       sensor->watermark = 1;
1745 +       sensor->old_data = 0;
1746 +
1747 +       switch (id) {
1748 +       case ST_ASM330LHH_ID_ACC:
1749 +               iio_dev->channels = st_asm330lhh_acc_channels;
1750 +               iio_dev->num_channels = ARRAY_SIZE(st_asm330lhh_acc_channels);
1751 +               iio_dev->name = "asm330lhh_accel";
1752 +               iio_dev->info = &st_asm330lhh_acc_info;
1753 +               sensor->batch_addr = ST_ASM330LHH_REG_FIFO_BATCH_ADDR;
1754 +               sensor->batch_mask = GENMASK(3, 0);
1755 +               sensor->offset = 0;
1756 +               break;
1757 +       case ST_ASM330LHH_ID_GYRO:
1758 +               iio_dev->channels = st_asm330lhh_gyro_channels;
1759 +               iio_dev->num_channels = ARRAY_SIZE(st_asm330lhh_gyro_channels);
1760 +               iio_dev->name = "asm330lhh_gyro";
1761 +               iio_dev->info = &st_asm330lhh_gyro_info;
1762 +               sensor->batch_addr = ST_ASM330LHH_REG_FIFO_BATCH_ADDR;
1763 +               sensor->batch_mask = GENMASK(7, 4);
1764 +               sensor->offset = 0;
1765 +               break;
1766 +       case ST_ASM330LHH_ID_TEMP:
1767 +               iio_dev->channels = st_asm330lhh_temp_channels;
1768 +               iio_dev->num_channels = ARRAY_SIZE(st_asm330lhh_temp_channels);
1769 +               iio_dev->name = "asm330lhh_temp";
1770 +               iio_dev->info = &st_asm330lhh_temp_info;
1771 +               sensor->offset = ST_ASM330LHH_OFFSET;
1772 +               break;
1773 +       default:
1774 +               return NULL;
1775 +       }
1776 +
1777 +       return iio_dev;
1778 +}
1779 +
1780 +int st_asm330lhh_probe(struct device *dev, int irq,
1781 +                      const struct st_asm330lhh_transfer_function *tf_ops)
1782 +{
1783 +       struct st_asm330lhh_hw *hw;
1784 +       int i, err;
1785 +
1786 +       hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
1787 +       if (!hw)
1788 +               return -ENOMEM;
1789 +
1790 +       dev_set_drvdata(dev, (void *)hw);
1791 +
1792 +       mutex_init(&hw->lock);
1793 +       mutex_init(&hw->fifo_lock);
1794 +
1795 +       hw->dev = dev;
1796 +       hw->irq = irq;
1797 +       hw->tf = tf_ops;
1798 +
1799 +       dev_info(hw->dev, "Ver: %s\n", ST_ASM330LHH_VERSION);
1800 +       err = st_asm330lhh_check_whoami(hw);
1801 +       if (err < 0)
1802 +               return err;
1803 +
1804 +       err = st_asm330lhh_init_device(hw);
1805 +       if (err < 0)
1806 +               return err;
1807 +
1808 +       for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1809 +               hw->iio_devs[i] = st_asm330lhh_alloc_iiodev(hw, i);
1810 +               if (!hw->iio_devs[i])
1811 +                       return -ENOMEM;
1812 +       }
1813 +
1814 +       if (hw->irq > 0) {
1815 +               err = st_asm330lhh_fifo_setup(hw);
1816 +               if (err < 0)
1817 +                       return err;
1818 +       }
1819 +
1820 +       for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1821 +               if (!hw->iio_devs[i])
1822 +                       continue;
1823 +
1824 +               err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
1825 +               if (err)
1826 +                       return err;
1827 +       }
1828 +
1829 +       dev_info(hw->dev, "probe ok\n");
1830 +
1831 +       return 0;
1832 +}
1833 +EXPORT_SYMBOL(st_asm330lhh_probe);
1834 +
1835 +static int __maybe_unused st_asm330lhh_suspend(struct device *dev)
1836 +{
1837 +       struct st_asm330lhh_hw *hw = dev_get_drvdata(dev);
1838 +       struct st_asm330lhh_sensor *sensor;
1839 +       int i, err = 0;
1840 +
1841 +       for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1842 +               if (!hw->iio_devs[i])
1843 +                       continue;
1844 +
1845 +               sensor = iio_priv(hw->iio_devs[i]);
1846 +
1847 +               if (!(hw->enable_mask & BIT(sensor->id)))
1848 +                       continue;
1849 +
1850 +               err = st_asm330lhh_set_odr(sensor, 0);
1851 +               if (err < 0)
1852 +                       return err;
1853 +       }
1854 +
1855 +       if (hw->enable_mask)
1856 +               err = st_asm330lhh_suspend_fifo(hw);
1857 +
1858 +       return err;
1859 +}
1860 +
1861 +static int __maybe_unused st_asm330lhh_resume(struct device *dev)
1862 +{
1863 +       struct st_asm330lhh_hw *hw = dev_get_drvdata(dev);
1864 +       struct st_asm330lhh_sensor *sensor;
1865 +       int i, err = 0;
1866 +
1867 +       for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1868 +               if (!hw->iio_devs[i])
1869 +                       continue;
1870 +
1871 +               sensor = iio_priv(hw->iio_devs[i]);
1872 +               if (!(hw->enable_mask & BIT(sensor->id)))
1873 +                       continue;
1874 +
1875 +               err = st_asm330lhh_set_odr(sensor, sensor->odr);
1876 +               if (err < 0)
1877 +                       return err;
1878 +       }
1879 +
1880 +       if (hw->enable_mask)
1881 +               err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_CONT);
1882 +
1883 +       return err;
1884 +}
1885 +
1886 +const struct dev_pm_ops st_asm330lhh_pm_ops = {
1887 +       SET_SYSTEM_SLEEP_PM_OPS(st_asm330lhh_suspend, st_asm330lhh_resume)
1888 +};
1889 +EXPORT_SYMBOL(st_asm330lhh_pm_ops);
1890 +
1891 +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
1892 +MODULE_DESCRIPTION("STMicroelectronics st_asm330lhh driver");
1893 +MODULE_LICENSE("GPL v2");
1894 +MODULE_VERSION(ST_ASM330LHH_VERSION);
1895 diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_i2c.c b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_i2c.c
1896 new file mode 100644
1897 index 0000000..4875097
1898 --- /dev/null
1899 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_i2c.c
1900 @@ -0,0 +1,94 @@
1901 +/*
1902 + * STMicroelectronics st_asm330lhh i2c driver
1903 + *
1904 + * Copyright 2018 STMicroelectronics Inc.
1905 + *
1906 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
1907 + *
1908 + * Licensed under the GPL-2.
1909 + */
1910 +
1911 +#include <linux/kernel.h>
1912 +#include <linux/module.h>
1913 +#include <linux/i2c.h>
1914 +#include <linux/slab.h>
1915 +#include <linux/of.h>
1916 +
1917 +#include "st_asm330lhh.h"
1918 +
1919 +static int st_asm330lhh_i2c_read(struct device *dev, u8 addr, int len, u8 *data)
1920 +{
1921 +       struct i2c_client *client = to_i2c_client(dev);
1922 +       struct i2c_msg msg[2];
1923 +
1924 +       msg[0].addr = client->addr;
1925 +       msg[0].flags = client->flags;
1926 +       msg[0].len = 1;
1927 +       msg[0].buf = &addr;
1928 +
1929 +       msg[1].addr = client->addr;
1930 +       msg[1].flags = client->flags | I2C_M_RD;
1931 +       msg[1].len = len;
1932 +       msg[1].buf = data;
1933 +
1934 +       return i2c_transfer(client->adapter, msg, 2);
1935 +}
1936 +
1937 +static int st_asm330lhh_i2c_write(struct device *dev, u8 addr, int len, u8 *data)
1938 +{
1939 +       struct i2c_client *client = to_i2c_client(dev);
1940 +       struct i2c_msg msg;
1941 +       u8 send[len + 1];
1942 +
1943 +       send[0] = addr;
1944 +       memcpy(&send[1], data, len * sizeof(u8));
1945 +
1946 +       msg.addr = client->addr;
1947 +       msg.flags = client->flags;
1948 +       msg.len = len + 1;
1949 +       msg.buf = send;
1950 +
1951 +       return i2c_transfer(client->adapter, &msg, 1);
1952 +}
1953 +
1954 +static const struct st_asm330lhh_transfer_function st_asm330lhh_transfer_fn = {
1955 +       .read = st_asm330lhh_i2c_read,
1956 +       .write = st_asm330lhh_i2c_write,
1957 +};
1958 +
1959 +static int st_asm330lhh_i2c_probe(struct i2c_client *client,
1960 +                               const struct i2c_device_id *id)
1961 +{
1962 +       return st_asm330lhh_probe(&client->dev, client->irq,
1963 +                               &st_asm330lhh_transfer_fn);
1964 +}
1965 +
1966 +static const struct of_device_id st_asm330lhh_i2c_of_match[] = {
1967 +       {
1968 +               .compatible = "st,asm330lhh",
1969 +       },
1970 +       {},
1971 +};
1972 +MODULE_DEVICE_TABLE(of, st_asm330lhh_i2c_of_match);
1973 +
1974 +static const struct i2c_device_id st_asm330lhh_i2c_id_table[] = {
1975 +       { ST_ASM330LHH_DEV_NAME },
1976 +       {},
1977 +};
1978 +MODULE_DEVICE_TABLE(i2c, st_asm330lhh_i2c_id_table);
1979 +
1980 +static struct i2c_driver st_asm330lhh_driver = {
1981 +       .driver = {
1982 +               .name = "st_asm330lhh_i2c",
1983 +               .pm = &st_asm330lhh_pm_ops,
1984 +               .of_match_table = of_match_ptr(st_asm330lhh_i2c_of_match),
1985 +       },
1986 +       .probe = st_asm330lhh_i2c_probe,
1987 +       .id_table = st_asm330lhh_i2c_id_table,
1988 +};
1989 +module_i2c_driver(st_asm330lhh_driver);
1990 +
1991 +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
1992 +MODULE_DESCRIPTION("STMicroelectronics st_asm330lhh i2c driver");
1993 +MODULE_LICENSE("GPL v2");
1994 +MODULE_VERSION(ST_ASM330LHH_VERSION);
1995 diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_spi.c b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_spi.c
1996 new file mode 100644
1997 index 0000000..07b8400
1998 --- /dev/null
1999 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_spi.c
2000 @@ -0,0 +1,109 @@
2001 +/*
2002 + * STMicroelectronics st_asm330lhh spi driver
2003 + *
2004 + * Copyright 2018 STMicroelectronics Inc.
2005 + *
2006 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
2007 + *
2008 + * Licensed under the GPL-2.
2009 + */
2010 +
2011 +#include <linux/kernel.h>
2012 +#include <linux/module.h>
2013 +#include <linux/spi/spi.h>
2014 +#include <linux/slab.h>
2015 +#include <linux/of.h>
2016 +
2017 +#include "st_asm330lhh.h"
2018 +
2019 +#define SENSORS_SPI_READ       BIT(7)
2020 +
2021 +static int st_asm330lhh_spi_read(struct device *dev, u8 addr, int len,
2022 +                              u8 *data)
2023 +{
2024 +       struct spi_device *spi = to_spi_device(dev);
2025 +       struct st_asm330lhh_hw *hw = spi_get_drvdata(spi);
2026 +       int err;
2027 +
2028 +       struct spi_transfer xfers[] = {
2029 +               {
2030 +                       .tx_buf = hw->tb.tx_buf,
2031 +                       .bits_per_word = 8,
2032 +                       .len = 1,
2033 +               },
2034 +               {
2035 +                       .rx_buf = hw->tb.rx_buf,
2036 +                       .bits_per_word = 8,
2037 +                       .len = len,
2038 +               }
2039 +       };
2040 +
2041 +       hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ;
2042 +
2043 +       err = spi_sync_transfer(spi, xfers,  ARRAY_SIZE(xfers));
2044 +       if (err < 0)
2045 +               return err;
2046 +
2047 +       memcpy(data, hw->tb.rx_buf, len * sizeof(u8));
2048 +
2049 +       return len;
2050 +}
2051 +
2052 +static int st_asm330lhh_spi_write(struct device *dev, u8 addr, int len,
2053 +                               u8 *data)
2054 +{
2055 +       struct st_asm330lhh_hw *hw;
2056 +       struct spi_device *spi;
2057 +
2058 +       if (len >= ST_ASM330LHH_TX_MAX_LENGTH)
2059 +               return -ENOMEM;
2060 +
2061 +       spi = to_spi_device(dev);
2062 +       hw = spi_get_drvdata(spi);
2063 +
2064 +       hw->tb.tx_buf[0] = addr;
2065 +       memcpy(&hw->tb.tx_buf[1], data, len);
2066 +
2067 +       return spi_write(spi, hw->tb.tx_buf, len + 1);
2068 +}
2069 +
2070 +static const struct st_asm330lhh_transfer_function st_asm330lhh_transfer_fn = {
2071 +       .read = st_asm330lhh_spi_read,
2072 +       .write = st_asm330lhh_spi_write,
2073 +};
2074 +
2075 +static int st_asm330lhh_spi_probe(struct spi_device *spi)
2076 +{
2077 +       return st_asm330lhh_probe(&spi->dev, spi->irq,
2078 +                               &st_asm330lhh_transfer_fn);
2079 +}
2080 +
2081 +static const struct of_device_id st_asm330lhh_spi_of_match[] = {
2082 +       {
2083 +               .compatible = "st,asm330lhh",
2084 +       },
2085 +       {},
2086 +};
2087 +MODULE_DEVICE_TABLE(of, st_asm330lhh_spi_of_match);
2088 +
2089 +static const struct spi_device_id st_asm330lhh_spi_id_table[] = {
2090 +       { ST_ASM330LHH_DEV_NAME },
2091 +       {},
2092 +};
2093 +MODULE_DEVICE_TABLE(spi, st_asm330lhh_spi_id_table);
2094 +
2095 +static struct spi_driver st_asm330lhh_driver = {
2096 +       .driver = {
2097 +               .name = "st_asm330lhh_spi",
2098 +               .pm = &st_asm330lhh_pm_ops,
2099 +               .of_match_table = of_match_ptr(st_asm330lhh_spi_of_match),
2100 +       },
2101 +       .probe = st_asm330lhh_spi_probe,
2102 +       .id_table = st_asm330lhh_spi_id_table,
2103 +};
2104 +module_spi_driver(st_asm330lhh_driver);
2105 +
2106 +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
2107 +MODULE_DESCRIPTION("STMicroelectronics st_asm330lhh spi driver");
2108 +MODULE_LICENSE("GPL v2");
2109 +MODULE_VERSION(ST_ASM330LHH_VERSION);
2110 --
2111 2.7.4
2112