Kingfisher: add multichannel audio, IMU driver

- Fix multichannel in rcar sound (optional compilation)
- add IMU driver
- enable new sensor drivers
- enalbe v3 usb3.0 firmware
- deploy Videobox dtb files

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
+