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
20 diff --git a/drivers/iio/imu/st_asm330lhh/Kconfig b/drivers/iio/imu/st_asm330lhh/Kconfig
22 index 0000000..0e8920e
24 +++ b/drivers/iio/imu/st_asm330lhh/Kconfig
26 +config IIO_ST_ASM330LHH
27 + tristate "STMicroelectronics ASM330LHH sensor"
28 + depends on (I2C || SPI)
30 + select IIO_KFIFO_BUF
31 + select IIO_ST_ASM330LHH_I2C if (I2C)
32 + select IIO_ST_ASM330LHH_SPI if (SPI_MASTER)
34 + Say yes here to build support for STMicroelectronics ASM330LHH imu
37 + To compile this driver as a module, choose M here: the module
38 + will be called st_asm330lhh.
40 +config IIO_ST_ASM330LHH_I2C
42 + depends on IIO_ST_ASM330LHH
44 +config IIO_ST_ASM330LHH_SPI
46 + depends on IIO_ST_ASM330LHH
48 diff --git a/drivers/iio/imu/st_asm330lhh/Makefile b/drivers/iio/imu/st_asm330lhh/Makefile
50 index 0000000..7af80de
52 +++ b/drivers/iio/imu/st_asm330lhh/Makefile
54 +st_asm330lhh-y := st_asm330lhh_core.o st_asm330lhh_buffer.o
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
61 index 0000000..d471530
63 +++ b/drivers/iio/imu/st_asm330lhh/README.md
68 + * Driver Integration details
69 + * Android SensorHAL integration
70 + * Linux SensorHAL integration
77 +This repository contains asm330lhh IMU STMicroelectronics MEMS sensor linux driver support for kernel version 4.14.
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.
81 +Asm330lhh IMU STM MEMS sensor support *I2C/SPI* digital interface. Please refer to [I2C][2] and [SPI][3] for detailed documentation.
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)
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
87 +Driver Integration details
88 +=====================
90 +In order to explain how to integrate Asm330lhh IMU STM sensor into the kernel, please consider the following example
92 +### Source code integration
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:
97 +> source "drivers/iio/imu/st_asm330lhh/Kconfig"
99 +> * Edit related Makefile (e.g. *drivers/iio/imu/Makefile*) adding the following line:
101 +> obj-y += st_asm330lhh/
103 +### Device Tree configuration
105 +> To enable driver probing, add the asm330lhh node to the platform device tree as described below.
107 +> **Required properties:**
109 +> *- compatible*: "st,asm330lhh"
111 +> *- reg*: the I2C address or SPI chip select the device will respond to
113 +> *- interrupt-parent*: phandle to the parent interrupt controller as documented in [interrupts][4]
115 +> *- interrupts*: interrupt mapping for IRQ as documented in [interrupts][4]
117 +>**Recommended properties for SPI bus usage:**
119 +> *- spi-max-frequency*: maximum SPI bus frequency as documented in [SPI][3]
121 +> **Optional properties:**
123 +> *- st,drdy-int-pin*: MEMS sensor interrupt line to use (default 1)
125 +> I2C example (based on Raspberry PI 3):
129 +> #address-cells = <0x1>;
130 +> #size-cells = <0x0>;
132 +> compatible = "st,asm330lhh";
134 +> interrupt-parent = <&gpio>;
135 +> interrupts = <26 IRQ_TYPE_EDGE_RISING>;
138 +> SPI example (based on Raspberry PI 3):
142 +> #address-cells = <0x1>;
143 +> #size-cells = <0x0>;
145 +> spi-max-frequency = <500000>;
146 +> compatible = "st,asm330lhh";
148 +> interrupt-parent = <&gpio>;
149 +> interrupts = <26 IRQ_TYPE_EDGE_RISING>;
152 +### Kernel configuration
154 +Configure kernel with *make menuconfig* (alternatively use *make xconfig* or *make qconfig*)
156 +> Device Drivers --->
157 +> <M> Industrial I/O support --->
158 +> Inertial measurement units --->
159 +> <M> STMicroelectronics ASM330LHH sensor --->
162 +Android SensorHAL integration
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*).
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:
170 + PRODUCT_PACKAGES += sensors.{TARGET_BOARD_PLATFORM}
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)
174 +To compile the SensorHAL_IIO just build AOSP source code from *$TOP* folder
177 + $ source build/envsetup.sh
178 + $ lunch <select target platform>
181 +The compiled library will be placed in *<AOSP_DIR\>/out/target/product/<board\>/system/vendor/lib/hw/sensor.{TARGET_BOARD_PLATFORM}.so*
183 +To configure sensor the Sensor HAL IIO use mm utility from HAL root folder
186 + $mm sensors-defconfig (default configuration)
187 + $mm sensors-menuconfig
190 + make -f Makefile_config sensors-defconfig (default configuration)
191 + make -f Makefile_config sensors-menuconfig
193 +Linux SensorHAL integration
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
200 +> export AOSP_DIR=<AOSP_DIR>
201 +> export ARCH=<your target architecture>
202 +> export CROSS_COMPILE=<toolchain for your target>
204 +then in *<AOSP_DIR\>/hardware/STMicroelectronics/SensorHAL_IIO* folder type
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
212 +Copyright (C) 2017 STMicroelectronics
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
218 +http://www.apache.org/licenses/LICENSE-2.0
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.
229 +[http://st.com](http://st.com)
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)
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)
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)
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)
242 +Copyright (C) 2017 STMicroelectronics
244 +This software is distributed under the GNU General Public License - see the accompanying COPYING file for more details.
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"
253 +Copyright (C) 2017 STMicroelectronics
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
259 +http://www.apache.org/licenses/LICENSE-2.0
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
268 index 0000000..a2f6e02
270 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
273 + * STMicroelectronics st_asm330lhh sensor driver
275 + * Copyright 2018 STMicroelectronics Inc.
277 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
279 + * Licensed under the GPL-2.
282 +#ifndef ST_ASM330LHH_H
283 +#define ST_ASM330LHH_H
285 +#include <linux/device.h>
286 +#include <linux/iio/iio.h>
288 +#define ST_ASM330LHH_REVISION "2.0.1"
289 +#define ST_ASM330LHH_PATCH "2"
291 +#define ST_ASM330LHH_VERSION "v" \
292 + ST_ASM330LHH_REVISION \
296 +#define ST_ASM330LHH_DEV_NAME "asm330lhh"
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
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
312 +#define ST_ASM330LHH_MAX_ODR 416
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)
319 +#define ST_ASM330LHH_CHANNEL(chan_type, addr, mod, ch2, scan_idx, \
322 + .type = chan_type, \
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, \
333 + .storagebits = sb, \
334 + .endianness = IIO_LE, \
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,
343 +#define ST_ASM330LHH_FLUSH_CHANNEL(dtype) \
347 + .scan_index = -1, \
349 + .event_spec = &st_asm330lhh_flush_event, \
350 + .num_event_specs = 1, \
353 +#define ST_ASM330LHH_RX_MAX_LENGTH 8
354 +#define ST_ASM330LHH_TX_MAX_LENGTH 8
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;
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);
366 +struct st_asm330lhh_reg {
371 +struct st_asm330lhh_odr {
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];
382 +struct st_asm330lhh_fs {
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 {
393 + struct st_asm330lhh_reg reg;
394 + struct st_asm330lhh_fs fs_avl[ST_ASM330LHH_FS_LIST_SIZE];
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,
404 +enum st_asm330lhh_fifo_mode {
405 + ST_ASM330LHH_FIFO_BYPASS = 0x0,
406 + ST_ASM330LHH_FIFO_CONT = 0x6,
410 + ST_ASM330LHH_HW_FLUSH,
411 + ST_ASM330LHH_HW_OPERATIONAL,
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
423 +struct st_asm330lhh_sensor {
424 + enum st_asm330lhh_sensor_id id;
425 + struct st_asm330lhh_hw *hw;
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.
465 +struct st_asm330lhh_hw {
466 + struct device *dev;
470 + struct mutex fifo_lock;
472 + enum st_asm330lhh_fifo_mode fifo_mode;
473 + unsigned long state;
489 + struct iio_dev *iio_devs[ST_ASM330LHH_ID_MAX];
491 + const struct st_asm330lhh_transfer_function *tf;
492 + struct st_asm330lhh_transfer_buffer tb;
495 +extern const struct dev_pm_ops st_asm330lhh_pm_ops;
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,
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,
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
521 index 0000000..c414a06
523 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
526 + * STMicroelectronics st_asm330lhh FIFO buffer library driver
528 + * Copyright 2018 STMicroelectronics Inc.
530 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
532 + * Licensed under the GPL-2.
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>
544 +#include "st_asm330lhh.h"
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
562 +#define ST_ASM330LHH_TS_DELTA_NS 25000ULL /* 25us/LSB */
564 +static inline s64 st_asm330lhh_get_time_ns(void)
566 + struct timespec ts;
568 + get_monotonic_boottime(&ts);
569 + return timespec_to_ns(&ts);
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)
579 + incr = div_s64((ST_ASM330LHH_EWMA_DIV - weight) * diff,
580 + ST_ASM330LHH_EWMA_DIV);
585 +static inline int st_asm330lhh_reset_hwts(struct st_asm330lhh_hw *hw)
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;
596 + return hw->tf->write(hw->dev, ST_ASM330LHH_REG_TS2_ADDR, sizeof(data),
600 +int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw,
601 + enum st_asm330lhh_fifo_mode fifo_mode)
605 + err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_FIFO_CTRL4_ADDR,
606 + ST_ASM330LHH_REG_FIFO_MODE_MASK,
611 + hw->fifo_mode = fifo_mode;
616 +static int st_asm330lhh_set_sensor_batching_odr(struct st_asm330lhh_sensor *sensor,
619 + struct st_asm330lhh_hw *hw = sensor->hw;
624 + err = st_asm330lhh_get_odr_val(sensor->id, sensor->odr, &data);
629 + return st_asm330lhh_write_with_mask(hw,
630 + sensor->batch_addr,
631 + sensor->batch_mask, data);
634 +static u16 st_asm330lhh_ts_odr(struct st_asm330lhh_hw *hw)
636 + struct st_asm330lhh_sensor *sensor;
640 + for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
641 + if (!hw->iio_devs[i])
644 + sensor = iio_priv(hw->iio_devs[i]);
645 + if (hw->enable_mask & BIT(sensor->id))
646 + odr = max_t(u16, odr, sensor->odr);
652 +static int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,
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;
662 + for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
663 + cur_sensor = iio_priv(hw->iio_devs[i]);
665 + if (!(hw->enable_mask & BIT(cur_sensor->id)))
668 + cur_watermark = (cur_sensor == sensor) ? watermark
669 + : cur_sensor->watermark;
671 + fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
674 + fifo_watermark = max_t(u16, fifo_watermark, 2);
675 + mutex_lock(&hw->lock);
677 + err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_FIFO_THL_ADDR + 1,
678 + sizeof(data), &data);
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);
689 + mutex_unlock(&hw->lock);
691 + return err < 0 ? err : 0;
694 +static inline void st_asm330lhh_sync_hw_ts(struct st_asm330lhh_hw *hw, s64 ts)
696 + s64 delta = ts - hw->hw_ts;
698 + hw->ts_offset = st_asm330lhh_ewma(hw->ts_offset, delta,
699 + ST_ASM330LHH_EWMA_LEVEL);
702 +static struct iio_dev *st_asm330lhh_get_iiodev_from_tag(struct st_asm330lhh_hw *hw,
705 + struct iio_dev *iio_dev;
708 + case ST_ASM330LHH_GYRO_TAG:
709 + iio_dev = hw->iio_devs[ST_ASM330LHH_ID_GYRO];
711 + case ST_ASM330LHH_ACC_TAG:
712 + iio_dev = hw->iio_devs[ST_ASM330LHH_ID_ACC];
722 +static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw)
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;
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;
733 + int ts_processed = 0;
734 + s64 hw_ts = 0ull, delta_hw_ts, cpu_timestamp;
736 + ts_irq = hw->ts - hw->delta_ts;
739 + err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_FIFO_DIFFL_ADDR,
740 + sizeof(fifo_status), (u8 *)&fifo_status);
744 + fifo_depth = le16_to_cpu(fifo_status) & ST_ASM330LHH_REG_FIFO_LEN_MASK;
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,
758 + for (i = 0; i < word_len; i += ST_ASM330LHH_FIFO_SAMPLE_SIZE) {
759 + ptr = &buf[i + ST_ASM330LHH_TAG_SIZE];
762 + if (tag == ST_ASM330LHH_TS_TAG) {
763 + hw->hw_val = get_unaligned_le32(ptr);
765 + /* check for timer rollover */
766 + if (hw->hw_val < hw->hw_val_old) {
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;
774 + div_s64(hw->delta_hw_ts * ST_ASM330LHH_MAX_ODR, hw->odr);
776 + hw->ts_offset = st_asm330lhh_ewma(hw->ts_offset, ts_irq -
777 + hw->hw_ts + ts_delta_offs, ST_ASM330LHH_EWMA_LEVEL);
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;
786 + hw->ts_offset + (hw->hw_ts + ts_delta_offs);
789 + hw->tsample + (ts_delta_hw_ts + ts_delta_offs);
791 + iio_dev = st_asm330lhh_get_iiodev_from_tag(hw, tag);
795 + sensor = iio_priv(iio_dev);
796 + if (sensor->std_samples < sensor->std_level) {
797 + sensor->std_samples++;
801 + sensor = iio_priv(iio_dev);
803 + /* Check if timestamp is in the future. */
804 + cpu_timestamp = st_asm330lhh_get_time_ns();
806 + /* Avoid samples in the future. */
807 + if (hw->tsample > cpu_timestamp)
808 + hw->tsample = cpu_timestamp;
810 + memcpy(iio_buf, ptr, ST_ASM330LHH_SAMPLE_SIZE);
811 + iio_push_to_buffers_with_timestamp(iio_dev,
816 + read_len += word_len;
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,
823 + ST_ASM330LHH_EWMA_LEVEL);
829 +ssize_t st_asm330lhh_get_max_watermark(struct device *dev,
830 + struct device_attribute *attr, char *buf)
832 + return sprintf(buf, "%d\n", ST_ASM330LHH_MAX_FIFO_DEPTH);
835 +ssize_t st_asm330lhh_get_watermark(struct device *dev,
836 + struct device_attribute *attr, char *buf)
838 + struct iio_dev *iio_dev = dev_get_drvdata(dev);
839 + struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
841 + return sprintf(buf, "%d\n", sensor->watermark);
844 +ssize_t st_asm330lhh_set_watermark(struct device *dev,
845 + struct device_attribute *attr,
846 + const char *buf, size_t size)
848 + struct iio_dev *iio_dev = dev_get_drvdata(dev);
849 + struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
852 + mutex_lock(&iio_dev->mlock);
853 + if (iio_buffer_enabled(iio_dev)) {
858 + err = kstrtoint(buf, 10, &val);
862 + err = st_asm330lhh_update_watermark(sensor, val);
866 + sensor->watermark = val;
869 + mutex_unlock(&iio_dev->mlock);
871 + return err < 0 ? err : size;
874 +ssize_t st_asm330lhh_flush_fifo(struct device *dev,
875 + struct device_attribute *attr,
876 + const char *buf, size_t size)
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;
885 + mutex_lock(&hw->fifo_lock);
886 + ts = st_asm330lhh_get_time_ns();
887 + hw->delta_ts = ts - hw->ts;
889 + set_bit(ST_ASM330LHH_HW_FLUSH, &hw->state);
891 + count = st_asm330lhh_read_fifo(hw);
893 + mutex_unlock(&hw->fifo_lock);
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());
903 +int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw)
907 + mutex_lock(&hw->fifo_lock);
909 + st_asm330lhh_read_fifo(hw);
910 + err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_BYPASS);
912 + mutex_unlock(&hw->fifo_lock);
917 +static int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable)
919 + struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
920 + struct st_asm330lhh_hw *hw = sensor->hw;
923 + mutex_lock(&hw->fifo_lock);
925 + err = st_asm330lhh_sensor_set_enable(sensor, enable);
929 + err = st_asm330lhh_set_sensor_batching_odr(sensor, enable);
933 + err = st_asm330lhh_update_watermark(sensor, sensor->watermark);
937 + hw->odr = st_asm330lhh_ts_odr(hw);
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);
947 + mutex_unlock(&hw->fifo_lock);
952 +static irqreturn_t st_asm330lhh_handler_irq(int irq, void *private)
954 + struct st_asm330lhh_hw *hw = (struct st_asm330lhh_hw *)private;
955 + s64 ts = st_asm330lhh_get_time_ns();
957 + hw->delta_ts = ts - hw->ts;
960 + return IRQ_WAKE_THREAD;
963 +static irqreturn_t st_asm330lhh_handler_thread(int irq, void *private)
965 + struct st_asm330lhh_hw *hw = (struct st_asm330lhh_hw *)private;
967 + mutex_lock(&hw->fifo_lock);
969 + st_asm330lhh_read_fifo(hw);
970 + clear_bit(ST_ASM330LHH_HW_FLUSH, &hw->state);
972 + mutex_unlock(&hw->fifo_lock);
974 + return IRQ_HANDLED;
977 +static int st_asm330lhh_buffer_preenable(struct iio_dev *iio_dev)
979 + return st_asm330lhh_update_fifo(iio_dev, true);
982 +static int st_asm330lhh_buffer_postdisable(struct iio_dev *iio_dev)
984 + return st_asm330lhh_update_fifo(iio_dev, false);
987 +static const struct iio_buffer_setup_ops st_asm330lhh_buffer_ops = {
988 + .preenable = st_asm330lhh_buffer_preenable,
989 + .postdisable = st_asm330lhh_buffer_postdisable,
992 +static int st_asm330lhh_fifo_init(struct st_asm330lhh_hw *hw)
994 + return st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_FIFO_CTRL4_ADDR,
995 + ST_ASM330LHH_REG_DEC_TS_MASK, 1);
998 +int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw)
1000 + struct device_node *np = hw->dev->of_node;
1001 + struct iio_buffer *buffer;
1002 + unsigned long irq_type;
1003 + bool irq_active_low;
1006 + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
1008 + switch (irq_type) {
1009 + case IRQF_TRIGGER_HIGH:
1010 + case IRQF_TRIGGER_RISING:
1011 + irq_active_low = false;
1013 + case IRQF_TRIGGER_LOW:
1014 + case IRQF_TRIGGER_FALLING:
1015 + irq_active_low = true;
1018 + dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
1022 + err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_HLACTIVE_ADDR,
1023 + ST_ASM330LHH_REG_HLACTIVE_MASK,
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);
1035 + irq_type |= IRQF_SHARED;
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,
1044 + dev_err(hw->dev, "failed to request trigger irq %d\n",
1049 + for (i = ST_ASM330LHH_ID_ACC; i < ST_ASM330LHH_ID_MAX; i++) {
1050 + if (!hw->iio_devs[i])
1053 + buffer = devm_iio_kfifo_allocate(hw->dev);
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;
1062 + return st_asm330lhh_fifo_init(hw);
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
1069 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
1072 + * STMicroelectronics st_asm330lhh sensor driver
1074 + * Copyright 2018 STMicroelectronics Inc.
1076 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
1078 + * Licensed under the GPL-2.
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>
1090 +#include <linux/platform_data/st_sensors_pdata.h>
1092 +#include "st_asm330lhh.h"
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)
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
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
1121 +#define ST_ASM330LHH_REG_LIR_ADDR 0x56
1122 +#define ST_ASM330LHH_REG_LIR_MASK BIT(0)
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)
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)
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)
1141 +struct st_asm330lhh_std_entry {
1146 +/* Minimal number of sample to be discarded */
1147 +struct st_asm330lhh_std_entry st_asm330lhh_std_table[] = {
1156 +static const struct st_asm330lhh_odr_table_entry st_asm330lhh_odr_table[] = {
1157 + [ST_ASM330LHH_ID_ACC] = {
1159 + .addr = ST_ASM330LHH_REG_CTRL1_XL_ADDR,
1160 + .mask = GENMASK(7, 4),
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 },
1170 + [ST_ASM330LHH_ID_GYRO] = {
1172 + .addr = ST_ASM330LHH_REG_CTRL2_G_ADDR,
1173 + .mask = GENMASK(7, 4),
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 },
1183 + [ST_ASM330LHH_ID_TEMP] = {
1184 + .odr_avl[0] = { 0, 0x00 },
1185 + .odr_avl[1] = { 52, 0x01 },
1189 +static const struct st_asm330lhh_fs_table_entry st_asm330lhh_fs_table[] = {
1190 + [ST_ASM330LHH_ID_ACC] = {
1192 + .addr = ST_ASM330LHH_REG_CTRL1_XL_ADDR,
1193 + .mask = GENMASK(3, 2),
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 },
1201 + [ST_ASM330LHH_ID_GYRO] = {
1203 + .addr = ST_ASM330LHH_REG_CTRL2_G_ADDR,
1204 + .mask = GENMASK(3, 0),
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 },
1214 + [ST_ASM330LHH_ID_TEMP] = {
1215 + .size = ST_ASM330LHH_FS_TEMP_LIST_SIZE,
1216 + .fs_avl[0] = { ST_ASM330LHH_TEMP_FS_GAIN, 0x0 },
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),
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),
1242 +static const struct iio_chan_spec st_asm330lhh_temp_channels[] = {
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),
1253 +int st_asm330lhh_write_with_mask(struct st_asm330lhh_hw *hw, u8 addr, u8 mask,
1259 + mutex_lock(&hw->lock);
1261 + err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
1263 + dev_err(hw->dev, "failed to read %02x register\n", addr);
1267 + data = (data & ~mask) | ((val << __ffs(mask)) & mask);
1269 + err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
1271 + dev_err(hw->dev, "failed to write %02x register\n", addr);
1274 + mutex_unlock(&hw->lock);
1279 +static int st_asm330lhh_check_whoami(struct st_asm330lhh_hw *hw)
1284 + err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_WHOAMI_ADDR, sizeof(data),
1287 + dev_err(hw->dev, "failed to read whoami register\n");
1291 + if (data != ST_ASM330LHH_WHOAMI_VAL) {
1292 + dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
1299 +static int st_asm330lhh_set_full_scale(struct st_asm330lhh_sensor *sensor,
1302 + enum st_asm330lhh_sensor_id id = sensor->id;
1306 + for (i = 0; i < st_asm330lhh_fs_table[id].size; i++)
1307 + if (st_asm330lhh_fs_table[id].fs_avl[i].gain == gain)
1310 + if (i == st_asm330lhh_fs_table[id].size)
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,
1321 + sensor->gain = gain;
1326 +int st_asm330lhh_get_odr_val(enum st_asm330lhh_sensor_id id, u16 odr, u8 *val)
1330 + for (i = 0; i < ST_ASM330LHH_ODR_LIST_SIZE; i++)
1331 + if (st_asm330lhh_odr_table[id].odr_avl[i].hz >= odr)
1334 + if (i == ST_ASM330LHH_ODR_LIST_SIZE)
1337 + *val = st_asm330lhh_odr_table[id].odr_avl[i].val;
1342 +static int st_asm330lhh_set_std_level(struct st_asm330lhh_sensor *sensor,
1347 + for (i = 0; i < ARRAY_SIZE(st_asm330lhh_std_table); i++)
1348 + if (st_asm330lhh_std_table[i].odr == odr)
1351 + if (i == ARRAY_SIZE(st_asm330lhh_std_table))
1354 + sensor->std_level = st_asm330lhh_std_table[i].val;
1355 + sensor->std_samples = 0;
1360 +static int st_asm330lhh_set_odr(struct st_asm330lhh_sensor *sensor, u16 odr)
1362 + struct st_asm330lhh_hw *hw = sensor->hw;
1365 + if (st_asm330lhh_get_odr_val(sensor->id, odr, &val) < 0)
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);
1373 +int st_asm330lhh_sensor_set_enable(struct st_asm330lhh_sensor *sensor,
1376 + u16 odr = enable ? sensor->odr : 0;
1379 + if (sensor->id != ST_ASM330LHH_ID_TEMP) {
1380 + err = st_asm330lhh_set_odr(sensor, odr);
1386 + sensor->hw->enable_mask |= BIT(sensor->id);
1388 + sensor->hw->enable_mask &= ~BIT(sensor->id);
1393 +static int st_asm330lhh_read_oneshot(struct st_asm330lhh_sensor *sensor,
1394 + u8 addr, int *val)
1399 + if (sensor->id == ST_ASM330LHH_ID_TEMP) {
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);
1408 + if (status & ST_ASM330LHH_REG_STATUS_TDA) {
1409 + err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
1414 + sensor->old_data = data;
1416 + data = sensor->old_data;
1418 + mutex_unlock(&sensor->hw->fifo_lock);
1421 + err = st_asm330lhh_sensor_set_enable(sensor, true);
1425 + delay = 1000000 / sensor->odr;
1426 + usleep_range(delay, 2 * delay);
1428 + err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
1433 + st_asm330lhh_sensor_set_enable(sensor, false);
1438 + return IIO_VAL_INT;
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)
1445 + struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
1449 + case IIO_CHAN_INFO_RAW:
1450 + mutex_lock(&iio_dev->mlock);
1451 + if (iio_buffer_enabled(iio_dev)) {
1453 + mutex_unlock(&iio_dev->mlock);
1456 + ret = st_asm330lhh_read_oneshot(sensor, ch->address, val);
1457 + mutex_unlock(&iio_dev->mlock);
1459 + case IIO_CHAN_INFO_OFFSET:
1460 + switch (ch->type) {
1462 + *val = sensor->offset;
1463 + ret = IIO_VAL_INT;
1469 + case IIO_CHAN_INFO_SAMP_FREQ:
1470 + *val = sensor->odr;
1471 + ret = IIO_VAL_INT;
1473 + case IIO_CHAN_INFO_SCALE:
1474 + switch (ch->type) {
1477 + *val2 = ST_ASM330LHH_TEMP_GAIN;
1478 + ret = IIO_VAL_FRACTIONAL;
1481 + case IIO_ANGL_VEL:
1483 + *val2 = sensor->gain;
1484 + ret = IIO_VAL_INT_PLUS_MICRO;
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)
1502 + struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
1505 + mutex_lock(&iio_dev->mlock);
1508 + case IIO_CHAN_INFO_SCALE:
1509 + err = st_asm330lhh_set_full_scale(sensor, val2);
1511 + case IIO_CHAN_INFO_SAMP_FREQ: {
1514 + err = st_asm330lhh_set_std_level(sensor, val);
1518 + err = st_asm330lhh_get_odr_val(sensor->id, val, &data);
1520 + sensor->odr = val;
1522 + err = st_asm330lhh_set_odr(sensor, sensor->odr);
1530 + mutex_unlock(&iio_dev->mlock);
1536 +st_asm330lhh_sysfs_sampling_frequency_avail(struct device *dev,
1537 + struct device_attribute *attr,
1540 + struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));
1541 + enum st_asm330lhh_sensor_id id = sensor->id;
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';
1552 +static ssize_t st_asm330lhh_sysfs_scale_avail(struct device *dev,
1553 + struct device_attribute *attr,
1556 + struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));
1557 + enum st_asm330lhh_sensor_id id = sensor->id;
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';
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);
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,
1590 +static const struct attribute_group st_asm330lhh_acc_attribute_group = {
1591 + .attrs = st_asm330lhh_acc_attributes,
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,
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,
1610 +static const struct attribute_group st_asm330lhh_gyro_attribute_group = {
1611 + .attrs = st_asm330lhh_gyro_attributes,
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,
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,
1630 +static const struct attribute_group st_asm330lhh_temp_attribute_group = {
1631 + .attrs = st_asm330lhh_temp_attributes,
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,
1641 +static const unsigned long st_asm330lhh_available_scan_masks[] = { 0x7, 0x0 };
1643 +static int st_asm330lhh_of_get_drdy_pin(struct st_asm330lhh_hw *hw, int *drdy_pin)
1645 + struct device_node *np = hw->dev->of_node;
1650 + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
1653 +static int st_asm330lhh_get_drdy_reg(struct st_asm330lhh_hw *hw, u8 *drdy_reg)
1655 + int err = 0, drdy_pin;
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;
1661 + pdata = (struct st_sensors_platform_data *)dev->platform_data;
1662 + drdy_pin = pdata ? pdata->drdy_int_pin : 1;
1665 + switch (drdy_pin) {
1667 + *drdy_reg = ST_ASM330LHH_REG_INT1_ADDR;
1670 + *drdy_reg = ST_ASM330LHH_REG_INT2_ADDR;
1673 + dev_err(hw->dev, "unsupported data ready pin\n");
1681 +static int st_asm330lhh_init_device(struct st_asm330lhh_hw *hw)
1686 + err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_RESET_ADDR,
1687 + ST_ASM330LHH_REG_RESET_MASK, 1);
1693 + /* latch interrupts */
1694 + err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_LIR_ADDR,
1695 + ST_ASM330LHH_REG_LIR_MASK, 1);
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);
1705 + err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_ROUNDING_ADDR,
1706 + ST_ASM330LHH_REG_ROUNDING_MASK, 3);
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);
1716 + /* enable FIFO watermak interrupt */
1717 + err = st_asm330lhh_get_drdy_reg(hw, &drdy_int_reg);
1721 + return st_asm330lhh_write_with_mask(hw, drdy_int_reg,
1722 + ST_ASM330LHH_REG_FIFO_FTH_IRQ_MASK, 1);
1725 +static struct iio_dev *st_asm330lhh_alloc_iiodev(struct st_asm330lhh_hw *hw,
1726 + enum st_asm330lhh_sensor_id id)
1728 + struct st_asm330lhh_sensor *sensor;
1729 + struct iio_dev *iio_dev;
1731 + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
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;
1739 + sensor = iio_priv(iio_dev);
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;
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;
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;
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;
1780 +int st_asm330lhh_probe(struct device *dev, int irq,
1781 + const struct st_asm330lhh_transfer_function *tf_ops)
1783 + struct st_asm330lhh_hw *hw;
1786 + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
1790 + dev_set_drvdata(dev, (void *)hw);
1792 + mutex_init(&hw->lock);
1793 + mutex_init(&hw->fifo_lock);
1799 + dev_info(hw->dev, "Ver: %s\n", ST_ASM330LHH_VERSION);
1800 + err = st_asm330lhh_check_whoami(hw);
1804 + err = st_asm330lhh_init_device(hw);
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])
1814 + if (hw->irq > 0) {
1815 + err = st_asm330lhh_fifo_setup(hw);
1820 + for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1821 + if (!hw->iio_devs[i])
1824 + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
1829 + dev_info(hw->dev, "probe ok\n");
1833 +EXPORT_SYMBOL(st_asm330lhh_probe);
1835 +static int __maybe_unused st_asm330lhh_suspend(struct device *dev)
1837 + struct st_asm330lhh_hw *hw = dev_get_drvdata(dev);
1838 + struct st_asm330lhh_sensor *sensor;
1841 + for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1842 + if (!hw->iio_devs[i])
1845 + sensor = iio_priv(hw->iio_devs[i]);
1847 + if (!(hw->enable_mask & BIT(sensor->id)))
1850 + err = st_asm330lhh_set_odr(sensor, 0);
1855 + if (hw->enable_mask)
1856 + err = st_asm330lhh_suspend_fifo(hw);
1861 +static int __maybe_unused st_asm330lhh_resume(struct device *dev)
1863 + struct st_asm330lhh_hw *hw = dev_get_drvdata(dev);
1864 + struct st_asm330lhh_sensor *sensor;
1867 + for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
1868 + if (!hw->iio_devs[i])
1871 + sensor = iio_priv(hw->iio_devs[i]);
1872 + if (!(hw->enable_mask & BIT(sensor->id)))
1875 + err = st_asm330lhh_set_odr(sensor, sensor->odr);
1880 + if (hw->enable_mask)
1881 + err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_CONT);
1886 +const struct dev_pm_ops st_asm330lhh_pm_ops = {
1887 + SET_SYSTEM_SLEEP_PM_OPS(st_asm330lhh_suspend, st_asm330lhh_resume)
1889 +EXPORT_SYMBOL(st_asm330lhh_pm_ops);
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
1899 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_i2c.c
1902 + * STMicroelectronics st_asm330lhh i2c driver
1904 + * Copyright 2018 STMicroelectronics Inc.
1906 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
1908 + * Licensed under the GPL-2.
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>
1917 +#include "st_asm330lhh.h"
1919 +static int st_asm330lhh_i2c_read(struct device *dev, u8 addr, int len, u8 *data)
1921 + struct i2c_client *client = to_i2c_client(dev);
1922 + struct i2c_msg msg[2];
1924 + msg[0].addr = client->addr;
1925 + msg[0].flags = client->flags;
1927 + msg[0].buf = &addr;
1929 + msg[1].addr = client->addr;
1930 + msg[1].flags = client->flags | I2C_M_RD;
1932 + msg[1].buf = data;
1934 + return i2c_transfer(client->adapter, msg, 2);
1937 +static int st_asm330lhh_i2c_write(struct device *dev, u8 addr, int len, u8 *data)
1939 + struct i2c_client *client = to_i2c_client(dev);
1940 + struct i2c_msg msg;
1944 + memcpy(&send[1], data, len * sizeof(u8));
1946 + msg.addr = client->addr;
1947 + msg.flags = client->flags;
1948 + msg.len = len + 1;
1951 + return i2c_transfer(client->adapter, &msg, 1);
1954 +static const struct st_asm330lhh_transfer_function st_asm330lhh_transfer_fn = {
1955 + .read = st_asm330lhh_i2c_read,
1956 + .write = st_asm330lhh_i2c_write,
1959 +static int st_asm330lhh_i2c_probe(struct i2c_client *client,
1960 + const struct i2c_device_id *id)
1962 + return st_asm330lhh_probe(&client->dev, client->irq,
1963 + &st_asm330lhh_transfer_fn);
1966 +static const struct of_device_id st_asm330lhh_i2c_of_match[] = {
1968 + .compatible = "st,asm330lhh",
1972 +MODULE_DEVICE_TABLE(of, st_asm330lhh_i2c_of_match);
1974 +static const struct i2c_device_id st_asm330lhh_i2c_id_table[] = {
1975 + { ST_ASM330LHH_DEV_NAME },
1978 +MODULE_DEVICE_TABLE(i2c, st_asm330lhh_i2c_id_table);
1980 +static struct i2c_driver st_asm330lhh_driver = {
1982 + .name = "st_asm330lhh_i2c",
1983 + .pm = &st_asm330lhh_pm_ops,
1984 + .of_match_table = of_match_ptr(st_asm330lhh_i2c_of_match),
1986 + .probe = st_asm330lhh_i2c_probe,
1987 + .id_table = st_asm330lhh_i2c_id_table,
1989 +module_i2c_driver(st_asm330lhh_driver);
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
1999 +++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_spi.c
2002 + * STMicroelectronics st_asm330lhh spi driver
2004 + * Copyright 2018 STMicroelectronics Inc.
2006 + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
2008 + * Licensed under the GPL-2.
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>
2017 +#include "st_asm330lhh.h"
2019 +#define SENSORS_SPI_READ BIT(7)
2021 +static int st_asm330lhh_spi_read(struct device *dev, u8 addr, int len,
2024 + struct spi_device *spi = to_spi_device(dev);
2025 + struct st_asm330lhh_hw *hw = spi_get_drvdata(spi);
2028 + struct spi_transfer xfers[] = {
2030 + .tx_buf = hw->tb.tx_buf,
2031 + .bits_per_word = 8,
2035 + .rx_buf = hw->tb.rx_buf,
2036 + .bits_per_word = 8,
2041 + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ;
2043 + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers));
2047 + memcpy(data, hw->tb.rx_buf, len * sizeof(u8));
2052 +static int st_asm330lhh_spi_write(struct device *dev, u8 addr, int len,
2055 + struct st_asm330lhh_hw *hw;
2056 + struct spi_device *spi;
2058 + if (len >= ST_ASM330LHH_TX_MAX_LENGTH)
2061 + spi = to_spi_device(dev);
2062 + hw = spi_get_drvdata(spi);
2064 + hw->tb.tx_buf[0] = addr;
2065 + memcpy(&hw->tb.tx_buf[1], data, len);
2067 + return spi_write(spi, hw->tb.tx_buf, len + 1);
2070 +static const struct st_asm330lhh_transfer_function st_asm330lhh_transfer_fn = {
2071 + .read = st_asm330lhh_spi_read,
2072 + .write = st_asm330lhh_spi_write,
2075 +static int st_asm330lhh_spi_probe(struct spi_device *spi)
2077 + return st_asm330lhh_probe(&spi->dev, spi->irq,
2078 + &st_asm330lhh_transfer_fn);
2081 +static const struct of_device_id st_asm330lhh_spi_of_match[] = {
2083 + .compatible = "st,asm330lhh",
2087 +MODULE_DEVICE_TABLE(of, st_asm330lhh_spi_of_match);
2089 +static const struct spi_device_id st_asm330lhh_spi_id_table[] = {
2090 + { ST_ASM330LHH_DEV_NAME },
2093 +MODULE_DEVICE_TABLE(spi, st_asm330lhh_spi_id_table);
2095 +static struct spi_driver st_asm330lhh_driver = {
2097 + .name = "st_asm330lhh_spi",
2098 + .pm = &st_asm330lhh_pm_ops,
2099 + .of_match_table = of_match_ptr(st_asm330lhh_spi_of_match),
2101 + .probe = st_asm330lhh_spi_probe,
2102 + .id_table = st_asm330lhh_spi_id_table,
2104 +module_spi_driver(st_asm330lhh_driver);
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);