summaryrefslogtreecommitdiffstats
path: root/meta-rcar-gen3-adas/recipes-kernel/linux/linux-renesas/0062-IIO-lsm9ds0-add-IMU-driver.patch
diff options
context:
space:
mode:
Diffstat (limited to 'meta-rcar-gen3-adas/recipes-kernel/linux/linux-renesas/0062-IIO-lsm9ds0-add-IMU-driver.patch')
-rw-r--r--meta-rcar-gen3-adas/recipes-kernel/linux/linux-renesas/0062-IIO-lsm9ds0-add-IMU-driver.patch972
1 files changed, 972 insertions, 0 deletions
diff --git a/meta-rcar-gen3-adas/recipes-kernel/linux/linux-renesas/0062-IIO-lsm9ds0-add-IMU-driver.patch b/meta-rcar-gen3-adas/recipes-kernel/linux/linux-renesas/0062-IIO-lsm9ds0-add-IMU-driver.patch
new file mode 100644
index 0000000..d3a329f
--- /dev/null
+++ b/meta-rcar-gen3-adas/recipes-kernel/linux/linux-renesas/0062-IIO-lsm9ds0-add-IMU-driver.patch
@@ -0,0 +1,972 @@
+From 4631208dd9557e0183acba14dec79318f9cabdc3 Mon Sep 17 00:00:00 2001
+From: Andrey Gusakov <andrey.gusakov@cogentembedded.com>
+Date: Wed, 7 Jun 2017 13:35:52 +0300
+Subject: [PATCH] IIO: lsm9ds0: add IMU driver
+
+Taken from:
+https://github.com/mpod/kernel-playground
+
+Signed-off-by: Andrey Gusakov <andrey.gusakov@cogentembedded.com>
+---
+ drivers/iio/imu/Kconfig | 11 +
+ drivers/iio/imu/Makefile | 2 +
+ drivers/iio/imu/lsm9ds0.c | 912 ++++++++++++++++++++++++++++++++++++++++++++++
+ 3 files changed, 925 insertions(+)
+ create mode 100644 drivers/iio/imu/lsm9ds0.c
+
+diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig
+index 1f1ad41ef881..063c09b8fc53 100644
+--- a/drivers/iio/imu/Kconfig
++++ b/drivers/iio/imu/Kconfig
+@@ -38,6 +38,17 @@ config KMX61
+ To compile this driver as module, choose M here: the module will
+ be called kmx61.
+
++config LSM9DS0
++ tristate "ST LSM9DS0 9-axis IMU"
++ depends on I2C
++ select IIO_BUFFER
++ select IIO_TRIGGERED_BUFFER
++ help
++ Say Y here if you want to build a driver for ST LSM9DS0
++ system-in-package featuring a 3D digital linear acceleration
++ sensor, a 3D digital angular rate sensor, and a 3D digital magnetic
++ sensor.
++
+ source "drivers/iio/imu/inv_mpu6050/Kconfig"
+
+ endmenu
+diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile
+index c71bcd30dc38..4de076d0766e 100644
+--- a/drivers/iio/imu/Makefile
++++ b/drivers/iio/imu/Makefile
+@@ -13,6 +13,8 @@ adis_lib-$(CONFIG_IIO_ADIS_LIB_BUFFER) += adis_trigger.o
+ adis_lib-$(CONFIG_IIO_ADIS_LIB_BUFFER) += adis_buffer.o
+ obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o
+
++obj-$(CONFIG_LSM9DS0) += lsm9ds0.o
++
+ obj-y += bmi160/
+ obj-y += inv_mpu6050/
+
+diff --git a/drivers/iio/imu/lsm9ds0.c b/drivers/iio/imu/lsm9ds0.c
+new file mode 100644
+index 000000000000..15e2671daef9
+--- /dev/null
++++ b/drivers/iio/imu/lsm9ds0.c
+@@ -0,0 +1,912 @@
++/*
++ * lsm9ds0_gyro.c
++ *
++ * Copyright (C) 2016 Matija Podravec <matija_podravec@fastmail.fm>
++ *
++ * This program is free software: you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License as published by
++ * the Free Software Foundation, either version 3 of the License, or
++ * (at your option) any later version.
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++
++ * You should have received a copy of the GNU General Public License
++ * along with this program. If not, see <http://www.gnu.org/licenses/>.
++ *
++ *
++ * Driver for ST LSM9DS0 gyroscope, accelerometer, and magnetometer sensor.
++ *
++ */
++
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/slab.h>
++#include <linux/i2c.h>
++#include <linux/bitops.h>
++#include <linux/iio/iio.h>
++#include <linux/iio/sysfs.h>
++#include <linux/iio/trigger_consumer.h>
++#include <linux/iio/kfifo_buf.h>
++
++#define LSM9DS0_WHO_AM_I_REG (0x0F)
++#define LSM9DS0_CTRL_REG1_G_REG (0x20)
++#define LSM9DS0_CTRL_REG2_G_REG (0x21)
++#define LSM9DS0_CTRL_REG3_G_REG (0x22)
++#define LSM9DS0_CTRL_REG4_G_REG (0x23)
++#define LSM9DS0_CTRL_REG5_G_REG (0x24)
++#define LSM9DS0_REFERENCE_G_REG (0x25)
++#define LSM9DS0_STATUS_REG_G_REG (0x27)
++#define LSM9DS0_OUT_X_L_G_REG (0x28)
++#define LSM9DS0_OUT_X_H_G_REG (0x29)
++#define LSM9DS0_OUT_Y_L_G_REG (0x2A)
++#define LSM9DS0_OUT_Y_H_G_REG (0x2B)
++#define LSM9DS0_OUT_Z_L_G_REG (0x2C)
++#define LSM9DS0_OUT_Z_H_G_REG (0x2D)
++#define LSM9DS0_FIFO_CTRL_REG_G_REG (0x2E)
++#define LSM9DS0_FIFO_SRC_REG_G_REG (0x2F)
++#define LSM9DS0_INT1_CFG_G_REG (0x30)
++#define LSM9DS0_INT1_SRC_G_REG (0x31)
++#define LSM9DS0_INT1_TSH_XH_G_REG (0x32)
++#define LSM9DS0_INT1_TSH_XL_G_REG (0x33)
++#define LSM9DS0_INT1_TSH_YH_G_REG (0x34)
++#define LSM9DS0_INT1_TSH_YL_G_REG (0x35)
++#define LSM9DS0_INT1_TSH_ZH_G_REG (0x36)
++#define LSM9DS0_INT1_TSH_ZL_G_REG (0x37)
++#define LSM9DS0_INT1_DURATION_G_REG (0x38)
++#define LSM9DS0_OUT_TEMP_L_XM_REG (0x05)
++#define LSM9DS0_OUT_TEMP_H_XM_REG (0x06)
++#define LSM9DS0_STATUS_REG_M_REG (0x07)
++#define LSM9DS0_OUT_X_L_M_REG (0x08)
++#define LSM9DS0_OUT_X_H_M_REG (0x09)
++#define LSM9DS0_OUT_Y_L_M_REG (0x0A)
++#define LSM9DS0_OUT_Y_H_M_REG (0x0B)
++#define LSM9DS0_OUT_Z_L_M_REG (0x0C)
++#define LSM9DS0_OUT_Z_H_M_REG (0x0D)
++#define LSM9DS0_INT_CTRL_REG_M_REG (0x12)
++#define LSM9DS0_INT_SRC_REG_M_REG (0x13)
++#define LSM9DS0_INT_THS_L_M_REG (0x14)
++#define LSM9DS0_INT_THS_H_M_REG (0x15)
++#define LSM9DS0_OFFSET_X_L_M_REG (0x16)
++#define LSM9DS0_OFFSET_X_H_M_REG (0x17)
++#define LSM9DS0_OFFSET_Y_L_M_REG (0x18)
++#define LSM9DS0_OFFSET_Y_H_M_REG (0x19)
++#define LSM9DS0_OFFSET_Z_L_M_REG (0x1A)
++#define LSM9DS0_OFFSET_Z_H_M_REG (0x1B)
++#define LSM9DS0_REFERENCE_X_REG (0x1C)
++#define LSM9DS0_REFERENCE_Y_REG (0x1D)
++#define LSM9DS0_REFERENCE_Z_REG (0x1E)
++#define LSM9DS0_CTRL_REG0_XM_REG (0x1F)
++#define LSM9DS0_CTRL_REG1_XM_REG (0x20)
++#define LSM9DS0_CTRL_REG2_XM_REG (0x21)
++#define LSM9DS0_CTRL_REG3_XM_REG (0x22)
++#define LSM9DS0_CTRL_REG4_XM_REG (0x23)
++#define LSM9DS0_CTRL_REG5_XM_REG (0x24)
++#define LSM9DS0_CTRL_REG6_XM_REG (0x25)
++#define LSM9DS0_CTRL_REG7_XM_REG (0x26)
++#define LSM9DS0_STATUS_REG_A_REG (0x27)
++#define LSM9DS0_OUT_X_L_A_REG (0x28)
++#define LSM9DS0_OUT_X_H_A_REG (0x29)
++#define LSM9DS0_OUT_Y_L_A_REG (0x2A)
++#define LSM9DS0_OUT_Y_H_A_REG (0x2B)
++#define LSM9DS0_OUT_Z_L_A_REG (0x2C)
++#define LSM9DS0_OUT_Z_H_A_REG (0x2D)
++#define LSM9DS0_FIFO_CTRL_REG_REG (0x2E)
++#define LSM9DS0_FIFO_SRC_REG_REG (0x2F)
++#define LSM9DS0_INT_GEN_1_REG_REG (0x30)
++#define LSM9DS0_INT_GEN_1_SRC_REG (0x31)
++#define LSM9DS0_INT_GEN_1_THS_REG (0x32)
++#define LSM9DS0_INT_GEN_1_DURATION_REG (0x33)
++#define LSM9DS0_INT_GEN_2_REG_REG (0x34)
++#define LSM9DS0_INT_GEN_2_SRC_REG (0x35)
++#define LSM9DS0_INT_GEN_2_THS_REG (0x36)
++#define LSM9DS0_INT_GEN_2_DURATION_REG (0x37)
++#define LSM9DS0_CLICK_CFG_REG (0x38)
++#define LSM9DS0_CLICK_SRC_REG (0x39)
++#define LSM9DS0_CLICK_THS_REG (0x3A)
++#define LSM9DS0_TIME_LIMIT_REG (0x3B)
++#define LSM9DS0_TIME_LATENCY_REG (0x3C)
++#define LSM9DS0_TIME_WINDOW_REG (0x3D)
++#define LSM9DS0_ACT_THS_REG (0x3E)
++#define LSM9DS0_ACT_DUR_REG (0x3F)
++
++#define LSM9DS0_GYRO_ODR_95HZ_VAL (0x00 << 6)
++#define LSM9DS0_GYRO_ODR_190HZ_VAL (0x01 << 6)
++#define LSM9DS0_GYRO_ODR_380HZ_VAL (0x02 << 6)
++#define LSM9DS0_GYRO_ODR_760HZ_VAL (0x03 << 6)
++
++#define LSM9DS0_ACCEL_POWER_DOWN (0x00 << 4)
++#define LSM9DS0_ACCEL_ODR_3_125HZ_VAL (0x01 << 4)
++#define LSM9DS0_ACCEL_ODR_6_25HZ_VAL (0x02 << 4)
++#define LSM9DS0_ACCEL_ODR_12_5HZ_VAL (0x03 << 4)
++#define LSM9DS0_ACCEL_ODR_25HZ_VAL (0x04 << 4)
++#define LSM9DS0_ACCEL_ODR_50HZ_VAL (0x05 << 4)
++#define LSM9DS0_ACCEL_ODR_100HZ_VAL (0x06 << 4)
++#define LSM9DS0_ACCEL_ODR_200HZ_VAL (0x07 << 4)
++#define LSM9DS0_ACCEL_ODR_400HZ_VAL (0x08 << 4)
++#define LSM9DS0_ACCEL_ODR_800HZ_VAL (0x09 << 4)
++#define LSM9DS0_ACCEL_ODR_1600HZ_VAL (0x0A << 4)
++
++#define LSM9DS0_ACCEL_FS_MASK (0x03 << 3)
++#define LSM9DS0_ACCEL_FS_2G_VAL (0x00 << 3)
++#define LSM9DS0_ACCEL_FS_4G_VAL (0x01 << 3)
++#define LSM9DS0_ACCEL_FS_6G_VAL (0x02 << 3)
++#define LSM9DS0_ACCEL_FS_8G_VAL (0x03 << 3)
++#define LSM9DS0_ACCEL_FS_16G_VAL (0x04 << 3)
++#define LSM9DS0_ACCEL_FS_2G_GAIN 61 /* ug/LSB */
++#define LSM9DS0_ACCEL_FS_4G_GAIN 122 /* ug/LSB */
++#define LSM9DS0_ACCEL_FS_6G_GAIN 183 /* ug/LSB */
++#define LSM9DS0_ACCEL_FS_8G_GAIN 244 /* ug/LSB */
++#define LSM9DS0_ACCEL_FS_16G_GAIN 732 /* ug/LSB */
++
++#define LSM9DS0_MAGN_ODR_3_125HZ_VAL (0x00 << 2)
++#define LSM9DS0_MAGN_ODR_6_25HZ_VAL (0x01 << 2)
++#define LSM9DS0_MAGN_ODR_12_5HZ_VAL (0x02 << 2)
++#define LSM9DS0_MAGN_ODR_25HZ_VAL (0x03 << 2)
++#define LSM9DS0_MAGN_ODR_50HZ_VAL (0x04 << 2)
++#define LSM9DS0_MAGN_ODR_100HZ_VAL (0x05 << 2)
++
++#define LSM9DS0_MAGN_FS_MASK (0x03 << 5)
++#define LSM9DS0_MAGN_FS_2GAUSS_VAL (0x00 << 5)
++#define LSM9DS0_MAGN_FS_4GAUSS_VAL (0x01 << 5)
++#define LSM9DS0_MAGN_FS_8GAUSS_VAL (0x02 << 5)
++#define LSM9DS0_MAGN_FS_12GAUSS_VAL (0x03 << 5)
++#define LSM9DS0_MAGN_FS_2GAUSS_GAIN 80 /* ugauss/LSB */
++#define LSM9DS0_MAGN_FS_4GAUSS_GAIN 160 /* ugauss/LSB */
++#define LSM9DS0_MAGN_FS_8GAUSS_GAIN 320 /* ugauss/LSB */
++#define LSM9DS0_MAGN_FS_12GAUSS_GAIN 480 /* ugauss/LSB */
++
++#define LSM9DS0_GYRO_FS_MASK (0x03 << 4)
++#define LSM9DS0_GYRO_FS_245DPS_VAL (0x00 << 4)
++#define LSM9DS0_GYRO_FS_500DPS_VAL (0x01 << 4)
++#define LSM9DS0_GYRO_FS_2000DPS_VAL (0x02 << 4)
++#define LSM9DS0_GYRO_FS_245DPS_GAIN 8750 /* udps/LSB */
++#define LSM9DS0_GYRO_FS_500DPS_GAIN 17500 /* udps/LSB */
++#define LSM9DS0_GYRO_FS_2000DPS_GAIN 70000 /* udps/LSB */
++
++#define LSM9DS0_GYRO_X_EN BIT(1)
++#define LSM9DS0_GYRO_Y_EN BIT(0)
++#define LSM9DS0_GYRO_Z_EN BIT(2)
++#define LSM9DS0_GYRO_POWER_DOWN (0x00 << 3)
++#define LSM9DS0_GYRO_NORMAL_MODE BIT(3)
++#define LSM9DS0_ACCEL_X_EN BIT(0)
++#define LSM9DS0_ACCEL_Y_EN BIT(1)
++#define LSM9DS0_ACCEL_Z_EN BIT(2)
++#define LSM9DS0_TEMP_EN BIT(7)
++#define LSM9DS0_MAGN_LOW_RES_VAL (0x00 << 5)
++#define LSM9DS0_MAGN_HIGH_RES_VAL (0x03 << 5)
++#define LSM9DS0_MAGN_POWER_DOWN (0x02)
++#define LSM9DS0_MAGN_CONT_CONV_MODE (0x00)
++#define LSM9DS0_MAGN_SINGLE_CONV_MODE (0x01)
++
++#define LSM9DS0_GYRO_ID 0xD4
++#define LSM9DS0_ACCEL_MAGN_ID 0x49
++
++enum { SCAN_INDEX_X, SCAN_INDEX_Y, SCAN_INDEX_Z };
++enum {
++ SCAN_INDEX_ACCEL_X, SCAN_INDEX_ACCEL_Y, SCAN_INDEX_ACCEL_Z,
++ SCAN_INDEX_MAGN_X, SCAN_INDEX_MAGN_Y, SCAN_INDEX_MAGN_Z
++};
++enum { GYRO, ACCEL_MAGN };
++
++struct lsm9ds0_data {
++ struct i2c_client *client;
++ struct mutex lock;
++ int sensor_type;
++ int gyro_scale;
++ int accel_scale;
++ int magn_scale;
++};
++
++struct sensor_fs_avl {
++ unsigned int num;
++ u8 value;
++ unsigned int gain;
++};
++
++static const struct sensor_fs_avl lsm9ds0_gyro_fs_avl[3] = {
++ {245, LSM9DS0_GYRO_FS_245DPS_VAL, LSM9DS0_GYRO_FS_245DPS_GAIN},
++ {500, LSM9DS0_GYRO_FS_500DPS_VAL, LSM9DS0_GYRO_FS_500DPS_GAIN},
++ {2000, LSM9DS0_GYRO_FS_2000DPS_VAL, LSM9DS0_GYRO_FS_2000DPS_GAIN},
++};
++
++static const struct sensor_fs_avl lsm9ds0_accel_fs_avl[5] = {
++ {2, LSM9DS0_ACCEL_FS_2G_VAL, LSM9DS0_ACCEL_FS_2G_GAIN},
++ {4, LSM9DS0_ACCEL_FS_4G_VAL, LSM9DS0_ACCEL_FS_4G_GAIN},
++ {6, LSM9DS0_ACCEL_FS_6G_VAL, LSM9DS0_ACCEL_FS_6G_GAIN},
++ {8, LSM9DS0_ACCEL_FS_8G_VAL, LSM9DS0_ACCEL_FS_8G_GAIN},
++ {16, LSM9DS0_ACCEL_FS_16G_VAL, LSM9DS0_ACCEL_FS_16G_GAIN},
++};
++
++static const struct sensor_fs_avl lsm9ds0_magn_fs_avl[4] = {
++ {2, LSM9DS0_MAGN_FS_2GAUSS_VAL, LSM9DS0_MAGN_FS_2GAUSS_GAIN},
++ {4, LSM9DS0_MAGN_FS_4GAUSS_VAL, LSM9DS0_MAGN_FS_4GAUSS_GAIN},
++ {8, LSM9DS0_MAGN_FS_8GAUSS_VAL, LSM9DS0_MAGN_FS_8GAUSS_GAIN},
++ {12, LSM9DS0_MAGN_FS_12GAUSS_VAL, LSM9DS0_MAGN_FS_12GAUSS_GAIN},
++};
++
++static ssize_t lsm9ds0_show_scale_avail(struct device *dev,
++ struct device_attribute *attr, char *buf)
++{
++ //struct iio_dev *indio_dev = dev_to_iio_dev(dev);
++ //struct lsm9ds0_data *data = iio_priv(indio_dev);
++ size_t len = 0;
++ int n;
++ const struct sensor_fs_avl (*avl)[];
++
++ if (strcmp(attr->attr.name, "in_gyro_scale_available") == 0) {
++ avl = &lsm9ds0_gyro_fs_avl;
++ n = ARRAY_SIZE(lsm9ds0_gyro_fs_avl);
++ } else if (strcmp(attr->attr.name, "in_accel_scale_available") == 0) {
++ avl = &lsm9ds0_accel_fs_avl;
++ n = ARRAY_SIZE(lsm9ds0_accel_fs_avl);
++ } else if (strcmp(attr->attr.name, "in_magn_scale_available") == 0) {
++ avl = &lsm9ds0_magn_fs_avl;
++ n = ARRAY_SIZE(lsm9ds0_magn_fs_avl);
++ } else {
++ return -EINVAL;
++ }
++
++ while (n-- > 0)
++ len += scnprintf(buf + len, PAGE_SIZE - len,
++ "0.%06u ", (*avl)[n].gain);
++ buf[len - 1] = '\n';
++
++ return len;
++}
++
++static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO,
++ lsm9ds0_show_scale_avail, NULL, 0);
++static IIO_DEVICE_ATTR(in_magn_scale_available, S_IRUGO,
++ lsm9ds0_show_scale_avail, NULL, 0);
++static IIO_DEVICE_ATTR(in_gyro_scale_available, S_IRUGO,
++ lsm9ds0_show_scale_avail, NULL, 0);
++
++static struct attribute *lsm9ds0_gyro_attributes[] = {
++ &iio_dev_attr_in_gyro_scale_available.dev_attr.attr,
++ NULL
++};
++
++static struct attribute *lsm9ds0_accel_magn_attributes[] = {
++ &iio_dev_attr_in_accel_scale_available.dev_attr.attr,
++ &iio_dev_attr_in_magn_scale_available.dev_attr.attr,
++ NULL
++};
++
++static const struct attribute_group lsm9ds0_gyro_group = {
++ .attrs = lsm9ds0_gyro_attributes,
++};
++
++static const struct attribute_group lsm9ds0_accel_magn_group = {
++ .attrs = lsm9ds0_accel_magn_attributes,
++};
++
++static const struct iio_buffer_setup_ops lsm9ds0_buffer_setup_ops = {
++ .postenable = &iio_triggered_buffer_postenable,
++ .predisable = &iio_triggered_buffer_predisable,
++};
++
++static const struct iio_chan_spec lsm9ds0_gyro_channels[] = {
++ {
++ .type = IIO_ANGL_VEL,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_X,
++ .scan_index = SCAN_INDEX_X,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_ANGL_VEL,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_Y,
++ .scan_index = SCAN_INDEX_Y,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_ANGL_VEL,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_Z,
++ .scan_index = SCAN_INDEX_Z,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ },
++ IIO_CHAN_SOFT_TIMESTAMP(3),
++};
++
++static const struct iio_chan_spec lsm9ds0_accel_magn_channels[] = {
++ {
++ .type = IIO_ACCEL,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_X,
++ .scan_index = SCAN_INDEX_ACCEL_X,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_ACCEL,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_Y,
++ .scan_index = SCAN_INDEX_ACCEL_Y,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_ACCEL,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_Z,
++ .scan_index = SCAN_INDEX_ACCEL_Z,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_MAGN,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_X,
++ .scan_index = SCAN_INDEX_MAGN_X,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_MAGN,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_Y,
++ .scan_index = SCAN_INDEX_MAGN_Y,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ }, {
++ .type = IIO_MAGN,
++ .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
++ .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
++ .modified = 1,
++ .channel2 = IIO_MOD_Z,
++ .scan_index = SCAN_INDEX_MAGN_Z,
++ .scan_type = {
++ .sign = 's',
++ .realbits = 16,
++ .storagebits = 16,
++ .shift = 0,
++ .endianness = IIO_LE,
++ },
++ },
++ IIO_CHAN_SOFT_TIMESTAMP(6),
++};
++
++static int lsm9ds0_read_measurements(struct i2c_client *client,
++ u8 reg_address, s16 *x, s16 *y, s16 *z)
++{
++ int ret;
++ u8 buf[6] = {0};
++
++ buf[0] = 0x80 | reg_address;
++ ret = i2c_master_send(client, buf, 1);
++ if (ret < 0)
++ return ret;
++
++ ret = i2c_master_recv(client, buf, 6);
++ if (ret < 0)
++ return ret;
++
++ *x = (buf[1] << 8) | buf[0];
++ *y = (buf[3] << 8) | buf[2];
++ *z = (buf[5] << 8) | buf[4];
++ return ret;
++}
++
++static int lsm9ds0_read_raw(struct iio_dev *iio_dev,
++ struct iio_chan_spec const *channel,
++ int *val, int *val2, long mask)
++{
++ struct lsm9ds0_data *data = iio_priv(iio_dev);
++ int err = 0;
++ s16 x = 0, y = 0, z = 0;
++ int scale = 0;
++
++ switch (mask) {
++ case IIO_CHAN_INFO_RAW:
++ mutex_lock(&data->lock);
++ switch (channel->type) {
++ case IIO_ANGL_VEL:
++ err = lsm9ds0_read_measurements(data->client,
++ LSM9DS0_OUT_X_L_G_REG, &x, &y, &z);
++ scale = data->gyro_scale;
++ break;
++ case IIO_ACCEL:
++ err = lsm9ds0_read_measurements(data->client,
++ LSM9DS0_OUT_X_L_A_REG, &x, &y, &z);
++ scale = data->accel_scale;
++ break;
++ case IIO_MAGN:
++ err = lsm9ds0_read_measurements(data->client,
++ LSM9DS0_OUT_X_L_M_REG, &x, &y, &z);
++ scale = data->magn_scale;
++ break;
++ default:
++ return -EINVAL;
++ }
++ mutex_unlock(&data->lock);
++ if (err < 0)
++ goto read_error;
++
++ switch (channel->channel2) {
++ case IIO_MOD_X:
++ *val = x;
++ break;
++ case IIO_MOD_Y:
++ *val = y;
++ break;
++ case IIO_MOD_Z:
++ *val = z;
++ break;
++ }
++ return IIO_VAL_INT;
++ case IIO_CHAN_INFO_SCALE:
++ *val = 0;
++ switch (channel->type) {
++ case IIO_ANGL_VEL:
++ *val2 = data->gyro_scale;
++ break;
++ case IIO_ACCEL:
++ *val2 = data->accel_scale;
++ break;
++ case IIO_MAGN:
++ *val2 = data->magn_scale;
++ break;
++ default:
++ return -EINVAL;
++ }
++ return IIO_VAL_INT_PLUS_MICRO;
++ default:
++ return -EINVAL;
++ }
++
++read_error:
++ return err;
++}
++
++static int lsm9ds0_write_config(struct i2c_client *client,
++ u8 reg_address, u8 mask, u8 value)
++{
++ u8 reg;
++ s32 ret;
++ ret = i2c_smbus_read_byte_data(client, reg_address);
++ if (ret < 0)
++ return -EINVAL;
++
++ reg = (u8)ret;
++ reg &= ~mask;
++ reg |= value;
++
++ ret = i2c_smbus_write_byte_data(client, reg_address, reg);
++
++ return ret;
++}
++
++static int lsm9ds0_write_raw(struct iio_dev *indio_dev,
++ struct iio_chan_spec const *channel,
++ int val, int val2, long mask)
++{
++ struct lsm9ds0_data *data = iio_priv(indio_dev);
++ struct i2c_client *client = data->client;
++ const struct sensor_fs_avl (*avl)[];
++ int n, i, ret;
++ u8 reg_address, reg_mask, new_value;
++ int *scale_in_data;
++
++ mutex_lock(&data->lock);
++ switch (mask) {
++ case IIO_CHAN_INFO_SCALE:
++ dev_info(&client->dev, "Vals %d %d\n", val, val2);
++ switch (channel->type) {
++ case IIO_ANGL_VEL:
++ avl = &lsm9ds0_gyro_fs_avl;
++ n = ARRAY_SIZE(lsm9ds0_gyro_fs_avl);
++ reg_address = LSM9DS0_CTRL_REG4_G_REG;
++ reg_mask = LSM9DS0_GYRO_FS_MASK;
++ scale_in_data = &(data->gyro_scale);
++ break;
++ case IIO_ACCEL:
++ avl = &lsm9ds0_accel_fs_avl;
++ n = ARRAY_SIZE(lsm9ds0_accel_fs_avl);
++ reg_address = LSM9DS0_CTRL_REG2_XM_REG;
++ reg_mask = LSM9DS0_ACCEL_FS_MASK;
++ scale_in_data = &(data->accel_scale);
++ break;
++ case IIO_MAGN:
++ avl = &lsm9ds0_magn_fs_avl;
++ n = ARRAY_SIZE(lsm9ds0_magn_fs_avl);
++ reg_address = LSM9DS0_CTRL_REG6_XM_REG;
++ reg_mask = LSM9DS0_MAGN_FS_MASK;
++ scale_in_data = &(data->magn_scale);
++ break;
++ default:
++ ret = -EINVAL;
++ goto done;
++ }
++ ret = -EINVAL;
++ for (i = 0; i < n; i++) {
++ if ((*avl)[i].gain == val2) {
++ ret = 0;
++ new_value = (*avl)[i].value;
++ break;
++ }
++ }
++ if (ret < 0)
++ goto done;
++
++ ret = lsm9ds0_write_config(client, reg_address, reg_mask, new_value);
++ if (ret < 0)
++ goto done;
++
++ *scale_in_data = (*avl)[i].gain;
++ break;
++ default:
++ ret = -EINVAL;
++ }
++
++done:
++ mutex_unlock(&data->lock);
++ return ret;
++}
++
++static irqreturn_t lsm9ds0_trigger_h(int irq, void *p)
++{
++ struct iio_poll_func *pf = p;
++ struct iio_dev *indio_dev = pf->indio_dev;
++ struct lsm9ds0_data *data = iio_priv(indio_dev);
++ u32 *buf_data;
++ int i, j;
++ s16 x1, y1, z1, x2, y2, z2;
++ int err;
++
++ buf_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
++ if (!buf_data)
++ goto done;
++
++ mutex_lock(&data->lock);
++ if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
++
++ if (data->sensor_type == GYRO) {
++ err = lsm9ds0_read_measurements(data->client,
++ LSM9DS0_OUT_X_L_G_REG, &x1, &y1, &z1);
++ if (err < 0)
++ goto free_buf;
++ } else if (data->sensor_type == ACCEL_MAGN) {
++ err = lsm9ds0_read_measurements(data->client,
++ LSM9DS0_OUT_X_L_A_REG, &x1, &y1, &z1);
++ if (err < 0)
++ goto free_buf;
++ err = lsm9ds0_read_measurements(data->client,
++ LSM9DS0_OUT_X_L_M_REG, &x2, &y2, &z2);
++ if (err < 0)
++ goto free_buf;
++ } else
++ goto free_buf;
++
++ for (i = 0, j = 0;
++ i < bitmap_weight(indio_dev->active_scan_mask, indio_dev->masklength);
++ i++, j++) {
++ j = find_next_bit(indio_dev->active_scan_mask, indio_dev->masklength, j);
++
++ if (data->sensor_type == GYRO) {
++ switch (j) {
++ case SCAN_INDEX_X:
++ buf_data[i] = x1;
++ break;
++ case SCAN_INDEX_Y:
++ buf_data[i] = y1;
++ break;
++ case SCAN_INDEX_Z:
++ buf_data[i] = z1;
++ break;
++ default:
++ break;
++ }
++ } else {
++ switch (j) {
++ case SCAN_INDEX_ACCEL_X:
++ buf_data[i] = x1;
++ break;
++ case SCAN_INDEX_ACCEL_Y:
++ buf_data[i] = y1;
++ break;
++ case SCAN_INDEX_ACCEL_Z:
++ buf_data[i] = z1;
++ break;
++ case SCAN_INDEX_MAGN_X:
++ buf_data[i] = x2;
++ break;
++ case SCAN_INDEX_MAGN_Y:
++ buf_data[i] = y2;
++ break;
++ case SCAN_INDEX_MAGN_Z:
++ buf_data[i] = z2;
++ break;
++ default:
++ break;
++ }
++ }
++ }
++ }
++
++ iio_push_to_buffers_with_timestamp(indio_dev, buf_data, iio_get_time_ns(indio_dev));
++
++free_buf:
++ kfree(buf_data);
++ mutex_unlock(&data->lock);
++
++done:
++ iio_trigger_notify_done(indio_dev->trig);
++
++ return IRQ_HANDLED;
++}
++
++static const struct iio_info lsm9ds0_gyro_info = {
++ .attrs = &lsm9ds0_gyro_group,
++ .read_raw = lsm9ds0_read_raw,
++ .write_raw = lsm9ds0_write_raw,
++ .driver_module = THIS_MODULE,
++};
++
++static const struct iio_info lsm9ds0_accel_magn_info = {
++ .attrs = &lsm9ds0_accel_magn_group,
++ .read_raw = lsm9ds0_read_raw,
++ .write_raw = lsm9ds0_write_raw,
++ .driver_module = THIS_MODULE,
++};
++
++static int lsm9ds0_gyro_init(struct i2c_client *client)
++{
++ int ret;
++ struct iio_dev *indio_dev;
++ struct lsm9ds0_data *data;
++
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG1_G_REG,
++ LSM9DS0_GYRO_NORMAL_MODE | LSM9DS0_GYRO_X_EN |
++ LSM9DS0_GYRO_Y_EN | LSM9DS0_GYRO_Z_EN);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 5.\n");
++ return ret;
++ }
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG4_G_REG,
++ LSM9DS0_GYRO_FS_245DPS_VAL);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 4.\n");
++ return ret;
++ }
++
++ indio_dev = i2c_get_clientdata(client);
++ data = iio_priv(indio_dev);
++
++ data->gyro_scale = LSM9DS0_GYRO_FS_245DPS_GAIN;
++
++ return 0;
++}
++
++static int lsm9ds0_accel_magn_init(struct i2c_client *client)
++{
++ int ret;
++ struct iio_dev *indio_dev;
++ struct lsm9ds0_data *data;
++
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG1_XM_REG,
++ LSM9DS0_ACCEL_ODR_100HZ_VAL | LSM9DS0_ACCEL_X_EN |
++ LSM9DS0_ACCEL_Y_EN | LSM9DS0_ACCEL_Z_EN);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 1.\n");
++ return ret;
++ }
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG5_XM_REG,
++ LSM9DS0_TEMP_EN | LSM9DS0_MAGN_HIGH_RES_VAL | LSM9DS0_MAGN_ODR_50HZ_VAL);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 5.\n");
++ return ret;
++ }
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG7_XM_REG,
++ LSM9DS0_MAGN_CONT_CONV_MODE);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 7.\n");
++ return ret;
++ }
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG2_XM_REG,
++ LSM9DS0_ACCEL_FS_2G_VAL);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 2.\n");
++ return ret;
++ }
++ ret = i2c_smbus_write_byte_data(client, LSM9DS0_CTRL_REG6_XM_REG,
++ LSM9DS0_MAGN_FS_2GAUSS_VAL);
++ if (ret < 0) {
++ dev_err(&client->dev, "Failed to write control register 6.\n");
++ return ret;
++ }
++
++ indio_dev = i2c_get_clientdata(client);
++ data = iio_priv(indio_dev);
++
++ data->accel_scale = LSM9DS0_ACCEL_FS_2G_GAIN;
++ data->magn_scale = LSM9DS0_MAGN_FS_2GAUSS_GAIN;
++
++ return 0;
++}
++
++static int lsm9ds0_probe(struct i2c_client *client,
++ const struct i2c_device_id *id)
++{
++ struct iio_dev *indio_dev;
++ struct lsm9ds0_data *data;
++ struct iio_buffer *buffer;
++ int sensor_type;
++ int ret;
++
++
++ if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
++ ret = -ENODEV;
++ goto error_ret;
++ }
++
++ ret = i2c_smbus_read_byte_data(client, LSM9DS0_WHO_AM_I_REG);
++ if (ret < 0) {
++ ret = -EINVAL;
++ goto error_ret;
++ }
++ if (ret == LSM9DS0_GYRO_ID) {
++ dev_info(&client->dev, "Gyroscope found.\n");
++ sensor_type = GYRO;
++ } else if (ret == LSM9DS0_ACCEL_MAGN_ID) {
++ dev_info(&client->dev, "Accelerometer and magnetometer found.\n");
++ sensor_type = ACCEL_MAGN;
++ } else {
++ dev_err(&client->dev, "No LSM9DS0 sensor found.\n");
++ ret = -ENODEV;
++ goto error_ret;
++ }
++
++ indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
++ if (!indio_dev) {
++ ret = -ENOMEM;
++ goto error_ret;
++ }
++
++ data = iio_priv(indio_dev);
++ mutex_init(&data->lock);
++ i2c_set_clientdata(client, indio_dev);
++ data->client = client;
++ data->sensor_type = sensor_type;
++
++ indio_dev->dev.parent = &client->dev;
++ indio_dev->name = dev_name(&client->dev);
++ indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_TRIGGERED;
++
++
++ if (sensor_type == GYRO) {
++ ret = lsm9ds0_gyro_init(client);
++ indio_dev->info = &lsm9ds0_gyro_info;
++ indio_dev->channels = lsm9ds0_gyro_channels;
++ indio_dev->num_channels = ARRAY_SIZE(lsm9ds0_gyro_channels);
++ } else {
++ ret = lsm9ds0_accel_magn_init(client);
++ indio_dev->info = &lsm9ds0_accel_magn_info;
++ indio_dev->channels = lsm9ds0_accel_magn_channels;
++ indio_dev->num_channels = ARRAY_SIZE(lsm9ds0_accel_magn_channels);
++ }
++ if (ret < 0)
++ goto error_free_device;
++
++ buffer = iio_kfifo_allocate();
++ if (!buffer) {
++ ret = -ENOMEM;
++ goto error_free_device;
++ }
++ iio_device_attach_buffer(indio_dev, buffer);
++ buffer->scan_timestamp = true;
++ indio_dev->setup_ops = &lsm9ds0_buffer_setup_ops;
++ indio_dev->pollfunc = iio_alloc_pollfunc(NULL,
++ &lsm9ds0_trigger_h,
++ IRQF_ONESHOT,
++ indio_dev,
++ "lsm9ds0_consumer%d",
++ indio_dev->id);
++ if (!indio_dev->pollfunc) {
++ ret = -ENOMEM;
++ goto error_free_buffer;
++ }
++
++ ret = iio_device_register(indio_dev);
++ if (ret < 0)
++ goto error_unconfigure_buffer;
++
++ return 0;
++
++error_unconfigure_buffer:
++ iio_dealloc_pollfunc(indio_dev->pollfunc);
++error_free_buffer:
++ iio_kfifo_free(indio_dev->buffer);
++error_free_device:
++ iio_device_free(indio_dev);
++error_ret:
++ return ret;
++}
++
++static int lsm9ds0_remove(struct i2c_client *client)
++{
++ struct iio_dev *indio_dev = i2c_get_clientdata(client);
++ iio_device_unregister(indio_dev);
++ iio_device_free(indio_dev);
++ dev_info(&client->dev, "Driver removed.");
++ return 0;
++}
++
++static const struct i2c_device_id lsm9ds0_id[] = {
++ { "lsm9ds0_gyro", 0 },
++ { "lsm9ds0_accel_magn", 0 },
++ { }
++};
++MODULE_DEVICE_TABLE(i2c, lsm9ds0_id);
++
++static struct i2c_driver lsm9ds0_driver = {
++ .driver = {
++ .name = "lsm9ds0",
++ .owner = THIS_MODULE,
++ },
++ .probe = lsm9ds0_probe,
++ .remove = lsm9ds0_remove,
++ .id_table = lsm9ds0_id,
++};
++module_i2c_driver(lsm9ds0_driver);
++
++MODULE_AUTHOR("Matija Podravec <matija_podravec@fastmail.fm>");
++MODULE_DESCRIPTION("LSM9DS0 gyroscope, accelerometer, and magnetometer sensor");
++MODULE_LICENSE("GPL");
+--
+2.13.0
+